Relintai/pandemonium_engine
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine.git synced 2025-02-02 14:25:55 +01:00
Code Issues Packages Projects Releases Wiki Activity

Removed bullet.

Browse Source
This commit is contained in:
Relintai 2022-03-23 13:28:35 +01:00
parent c9ad789c17
commit d1587aaa6b
731 changed files with 0 additions and 266845 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
SConstruct
View File

@ -164,7 +164,6 @@ opts.Add(
)
# 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_enet", "Use the built-in ENet library", True))
opts.Add(BoolVariable("builtin_freetype", "Use the built-in FreeType library", True))

220
modules/bullet/SCsub
View File

@ -1,220 +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.
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])
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)

340
modules/bullet/area_bullet.cpp
View File

@ -1,340 +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>
/**
@author AndreaCatania
*/
AreaBullet::AreaBullet() :
RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_AREA),
monitorable(true),
spOv_mode(PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED),
spOv_gravityPoint(false),
spOv_gravityPointDistanceScale(0),
spOv_gravityPointAttenuation(1),
spOv_gravityVec(0, -1, 0),
spOv_gravityMag(10),
spOv_linearDump(0.1),
spOv_angularDump(0.1),
spOv_priority(0),
isScratched(false) {
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, PhysicsServer::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, PhysicsServer::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(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, PhysicsServer::AreaBodyStatus p_status) {
InOutEventCallback &event = eventsCallbacks[static_cast<int>(p_overlapping_shape.other_object->getType())];
Object *areaGodoObject = ObjectDB::get_instance(event.event_callback_id);
if (!areaGodoObject) {
event.event_callback_id = 0;
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
Variant::CallError outResp;
areaGodoObject->call(event.event_callback_method, (const Variant **)call_event_res_ptr, 5, 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(i);
}
}
}
void AreaBullet::clear_overlaps() {
for (int i = 0; i < overlapping_shapes.size(); i++) {
call_event(overlapping_shapes[i], PhysicsServer::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(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
switch (p_param) {
case PhysicsServer::AREA_PARAM_GRAVITY:
set_spOv_gravityMag(p_value);
break;
case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
set_spOv_gravityVec(p_value);
break;
case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
set_spOv_linearDump(p_value);
break;
case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
set_spOv_angularDump(p_value);
break;
case PhysicsServer::AREA_PARAM_PRIORITY:
set_spOv_priority(p_value);
break;
case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
set_spOv_gravityPoint(p_value);
break;
case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
set_spOv_gravityPointDistanceScale(p_value);
break;
case PhysicsServer::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(PhysicsServer::AreaParameter p_param) const {
switch (p_param) {
case PhysicsServer::AREA_PARAM_GRAVITY:
return spOv_gravityMag;
case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
return spOv_gravityVec;
case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
return spOv_linearDump;
case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
return spOv_angularDump;
case PhysicsServer::AREA_PARAM_PRIORITY:
return spOv_priority;
case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
return spOv_gravityPoint;
case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
return spOv_gravityPointDistanceScale;
case PhysicsServer::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, ObjectID p_id, const StringName &p_method) {
InOutEventCallback &ev = eventsCallbacks[static_cast<int>(p_callbackObjectType)];
ev.event_callback_id = p_id;
ev.event_callback_method = p_method;
/// Set if monitoring
if (eventsCallbacks[0].event_callback_id || eventsCallbacks[1].event_callback_id) {
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_id;
}
void AreaBullet::on_enter_area(AreaBullet *p_area) {
}
void AreaBullet::on_exit_area(AreaBullet *p_area) {
CollisionObjectBullet::on_exit_area(p_area);
}

167
modules/bullet/area_bullet.h
View File

@ -1,167 +0,0 @@
#ifndef AREABULLET_H
#define AREABULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "collision_object_bullet.h"
#include "core/vector.h"
#include "servers/physics_server.h"
#include "space_bullet.h"
/**
@author AndreaCatania
*/
class btGhostObject;
class AreaBullet : public RigidCollisionObjectBullet {
public:
struct InOutEventCallback {
ObjectID event_callback_id;
StringName event_callback_method;
InOutEventCallback() :
event_callback_id(0) {}
};
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;
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;
PhysicsServer::AreaSpaceOverrideMode spOv_mode;
bool spOv_gravityPoint;
real_t spOv_gravityPointDistanceScale;
real_t spOv_gravityPointAttenuation;
Vector3 spOv_gravityVec;
real_t spOv_gravityMag;
real_t spOv_linearDump;
real_t spOv_angularDump;
int spOv_priority;
bool isScratched;
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(PhysicsServer::AreaSpaceOverrideMode p_mode) { spOv_mode = p_mode; }
_FORCE_INLINE_ PhysicsServer::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, PhysicsServer::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(PhysicsServer::AreaParameter p_param, const Variant &p_value);
Variant get_param(PhysicsServer::AreaParameter p_param) const;
void set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method);
bool has_event_callback(Type p_callbackObjectType);
virtual void on_enter_area(AreaBullet *p_area);
virtual void on_exit_area(AreaBullet *p_area);
};
#endif

106
modules/bullet/btRayShape.cpp
View File

@ -1,106 +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>
/**
@author AndreaCatania
*/
btRayShape::btRayShape(btScalar length) :
btConvexInternalShape(),
m_shapeAxis(0, 0, 1) {
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);
}

95
modules/bullet/btRayShape.h
View File

@ -1,95 +0,0 @@
#ifndef BTRAYSHAPE_H
#define BTRAYSHAPE_H
/*************************************************************************/
/* 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
#include <BulletCollision/CollisionShapes/btConvexInternalShape.h>
/**
@author AndreaCatania
*/
/// Ray shape around z axis
ATTRIBUTE_ALIGNED16(class)
btRayShape : public btConvexInternalShape {
btScalar m_length;
bool slipsOnSlope;
/// The default axis is the z
btVector3 m_shapeAxis;
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

1583
modules/bullet/bullet_physics_server.cpp
View File

File diff suppressed because it is too large Load Diff

411
modules/bullet/bullet_physics_server.h
View File

@ -1,411 +0,0 @@
#ifndef BULLET_PHYSICS_SERVER_H
#define BULLET_PHYSICS_SERVER_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "area_bullet.h"
#include "core/rid.h"
#include "joint_bullet.h"
#include "rigid_body_bullet.h"
#include "servers/physics_server.h"
#include "shape_bullet.h"
#include "soft_body_bullet.h"
#include "space_bullet.h"
/**
@author AndreaCatania
*/
class BulletPhysicsServer : public PhysicsServer {
GDCLASS(BulletPhysicsServer, PhysicsServer);
friend class BulletPhysicsDirectSpaceState;
bool active;
char active_spaces_count;
Vector<SpaceBullet *> active_spaces;
mutable RID_Owner<SpaceBullet> space_owner;
mutable RID_Owner<ShapeBullet> shape_owner;
mutable RID_Owner<AreaBullet> area_owner;
mutable RID_Owner<RigidBodyBullet> rigid_body_owner;
mutable RID_Owner<SoftBodyBullet> soft_body_owner;
mutable RID_Owner<JointBullet> joint_owner;
protected:
static void _bind_methods();
public:
BulletPhysicsServer();
~BulletPhysicsServer();
_FORCE_INLINE_ RID_Owner<SpaceBullet> *get_space_owner() {
return &space_owner;
}
_FORCE_INLINE_ RID_Owner<ShapeBullet> *get_shape_owner() {
return &shape_owner;
}
_FORCE_INLINE_ RID_Owner<AreaBullet> *get_area_owner() {
return &area_owner;
}
_FORCE_INLINE_ RID_Owner<RigidBodyBullet> *get_rigid_body_owner() {
return &rigid_body_owner;
}
_FORCE_INLINE_ RID_Owner<SoftBodyBullet> *get_soft_body_owner() {
return &soft_body_owner;
}
_FORCE_INLINE_ RID_Owner<JointBullet> *get_joint_owner() {
return &joint_owner;
}
/* SHAPE API */
virtual RID shape_create(ShapeType p_shape);
virtual void shape_set_data(RID p_shape, const Variant &p_data);
virtual ShapeType shape_get_type(RID p_shape) const;
virtual Variant shape_get_data(RID p_shape) const;
virtual void shape_set_margin(RID p_shape, real_t p_margin);
virtual real_t shape_get_margin(RID p_shape) const;
/// Not supported
virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
/// Not supported
virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
/* SPACE API */
virtual RID space_create();
virtual void space_set_active(RID p_space, bool p_active);
virtual bool space_is_active(RID p_space) const;
/// Not supported
virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value);
/// Not supported
virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const;
virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space);
virtual void space_set_debug_contacts(RID p_space, int p_max_contacts);
virtual Vector<Vector3> space_get_contacts(RID p_space) const;
virtual int space_get_contact_count(RID p_space) const;
/* 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();
virtual void area_set_space(RID p_area, RID p_space);
virtual RID area_get_space(RID p_area) const;
virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false);
virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform);
virtual int area_get_shape_count(RID p_area) const;
virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const;
virtual void area_remove_shape(RID p_area, int p_shape_idx);
virtual void area_clear_shapes(RID p_area);
virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled);
virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id);
virtual ObjectID area_get_object_instance_id(RID p_area) const;
/// 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);
virtual Variant area_get_param(RID p_area, AreaParameter p_param) const;
virtual void area_set_transform(RID p_area, const Transform &p_transform);
virtual Transform area_get_transform(RID p_area) const;
virtual void area_set_collision_mask(RID p_area, uint32_t p_mask);
virtual void area_set_collision_layer(RID p_area, uint32_t p_layer);
virtual void area_set_monitorable(RID p_area, bool p_monitorable);
virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
virtual void area_set_ray_pickable(RID p_area, bool p_enable);
virtual bool area_is_ray_pickable(RID p_area) const;
/* RIGID BODY API */
virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false);
virtual void body_set_space(RID p_body, RID p_space);
virtual RID body_get_space(RID p_body) const;
virtual void body_set_mode(RID p_body, BodyMode p_mode);
virtual BodyMode body_get_mode(RID p_body) const;
virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform(), bool p_disabled = false);
// Not supported, Please remove and add new shape
virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform);
virtual int body_get_shape_count(RID p_body) const;
virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
virtual void body_remove_shape(RID p_body, int p_shape_idx);
virtual void body_clear_shapes(RID p_body);
// Used for Rigid and Soft Bodies
virtual void body_attach_object_instance_id(RID p_body, uint32_t p_id);
virtual uint32_t body_get_object_instance_id(RID p_body) const;
virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable);
virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
virtual void body_set_collision_layer(RID p_body, uint32_t p_layer);
virtual uint32_t body_get_collision_layer(RID p_body) const;
virtual void body_set_collision_mask(RID p_body, uint32_t p_mask);
virtual uint32_t body_get_collision_mask(RID p_body) const;
/// This is not supported by physics server
virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
/// This is not supported by physics server
virtual uint32_t body_get_user_flags(RID p_body) const;
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
virtual float body_get_param(RID p_body, BodyParameter p_param) const;
virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin);
virtual real_t body_get_kinematic_safe_margin(RID p_body) const;
virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
virtual Variant body_get_state(RID p_body, BodyState p_state) const;
virtual void body_set_applied_force(RID p_body, const Vector3 &p_force);
virtual Vector3 body_get_applied_force(RID p_body) const;
virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque);
virtual Vector3 body_get_applied_torque(RID p_body) const;
virtual void body_add_central_force(RID p_body, const Vector3 &p_force);
virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos);
virtual void body_add_torque(RID p_body, const Vector3 &p_torque);
virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse);
virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse);
virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock);
virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const;
virtual void body_add_collision_exception(RID p_body, RID p_body_b);
virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
virtual int body_get_max_contacts_reported(RID p_body) const;
virtual void body_set_contacts_reported_depth_threshold(RID p_body, float p_threshold);
virtual float body_get_contacts_reported_depth_threshold(RID p_body) const;
virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
virtual bool body_is_omitting_force_integration(RID p_body) const;
virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
virtual void body_set_ray_pickable(RID p_body, bool p_enable);
virtual bool body_is_ray_pickable(RID p_body) const;
// this function only works on physics process, errors and returns null otherwise
virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
virtual bool body_test_motion(RID p_body, const Transform &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>());
virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001);
/* SOFT BODY API */
virtual RID soft_body_create(bool p_init_sleeping = false);
virtual void soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_server_handler);
virtual void soft_body_set_space(RID p_body, RID p_space);
virtual RID soft_body_get_space(RID p_body) const;
virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh);
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer);
virtual uint32_t soft_body_get_collision_layer(RID p_body) const;
virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask);
virtual uint32_t soft_body_get_collision_mask(RID p_body) const;
virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b);
virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b);
virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const;
/// Special function. This function has bad performance
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform);
virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const;
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable);
virtual bool soft_body_is_ray_pickable(RID p_body) const;
virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision);
virtual int soft_body_get_simulation_precision(RID p_body);
virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass);
virtual real_t soft_body_get_total_mass(RID p_body);
virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness);
virtual real_t soft_body_get_linear_stiffness(RID p_body);
virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness);
virtual real_t soft_body_get_areaAngular_stiffness(RID p_body);
virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness);
virtual real_t soft_body_get_volume_stiffness(RID p_body);
virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient);
virtual real_t soft_body_get_pressure_coefficient(RID p_body);
virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient);
virtual real_t soft_body_get_pose_matching_coefficient(RID p_body);
virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient);
virtual real_t soft_body_get_damping_coefficient(RID p_body);
virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient);
virtual real_t soft_body_get_drag_coefficient(RID p_body);
virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position);
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index);
virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const;
virtual void soft_body_remove_all_pinned_points(RID p_body);
virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin);
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index);
/* JOINT API */
virtual JointType joint_get_type(RID p_joint) const;
virtual void joint_set_solver_priority(RID p_joint, int p_priority);
virtual int joint_get_solver_priority(RID p_joint) const;
virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable);
virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const;
virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value);
virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const;
virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A);
virtual Vector3 pin_joint_get_local_a(RID p_joint) const;
virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B);
virtual Vector3 pin_joint_get_local_b(RID p_joint) const;
virtual RID joint_create_hinge(RID p_body_A, const Transform &p_hinge_A, RID p_body_B, const Transform &p_hinge_B);
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);
virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value);
virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const;
virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value);
virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const;
/// Reference frame is A
virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value);
virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const;
/// Reference frame is A
virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value);
virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const;
/// Reference frame is A
virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, float p_value);
virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param);
virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable);
virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag);
/* MISC */
virtual void free(RID p_rid);
virtual void set_active(bool p_active) {
active = p_active;
}
static bool singleton_isActive() {
return static_cast<BulletPhysicsServer *>(get_singleton())->active;
}
bool isActive() {
return active;
}
virtual void init();
virtual void step(float p_deltaTime);
virtual void flush_queries();
virtual void finish();
virtual bool is_flushing_queries() const { return false; }
virtual void set_collision_iterations(int p_iterations);
virtual int get_process_info(ProcessInfo p_info);
CollisionObjectBullet *get_collision_object(RID p_object) const;
RigidCollisionObjectBullet *get_rigid_collision_object(RID p_object) const;
/// Internal APIs
public:
};
#endif

155
modules/bullet/bullet_types_converter.cpp
View File

@ -1,155 +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"
/**
@author AndreaCatania
*/
// ++ 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, Transform &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(Transform 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 othogonal 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]);
}

61
modules/bullet/bullet_types_converter.h
View File

@ -1,61 +0,0 @@
#ifndef BULLET_TYPES_CONVERTER_H
#define BULLET_TYPES_CONVERTER_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/math/basis.h"
#include "core/math/transform.h"
#include "core/math/vector3.h"
#include "core/typedefs.h"
#include <LinearMath/btMatrix3x3.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btVector3.h>
/**
@author AndreaCatania
*/
// 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, Transform &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(Transform const &inVal, btTransform &outVal);
extern void UNSCALE_BT_BASIS(btTransform &scaledBasis);
#endif

45
modules/bullet/bullet_utilities.h
View File

@ -1,45 +0,0 @@
#ifndef BULLET_UTILITIES_H
#define BULLET_UTILITIES_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
/**
@author AndreaCatania
*/
#define bulletnew(cl) \
new cl
#define bulletdelete(cl) \
{ \
delete cl; \
cl = NULL; \
}
#endif

415
modules/bullet/collision_object_bullet.cpp
View File

@ -1,415 +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>
/**
@author AndreaCatania
*/
// 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 Transform &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() == PhysicsServer::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),
instance_id(0),
collisionLayer(0),
collisionMask(0),
collisionsEnabled(true),
m_isStatic(false),
ray_pickable(false),
bt_collision_object(nullptr),
body_scale(1., 1., 1.),
force_shape_reset(false),
space(nullptr) {}
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 Transform &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);
}
Transform CollisionObjectBullet::get_transform() const {
Transform 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(Type p_type) :
CollisionObjectBullet(p_type),
mainShape(nullptr) {
}
RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
remove_all_shapes(true, true);
if (mainShape && mainShape->isCompound()) {
bulletdelete(mainShape);
}
}
void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform &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(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(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 Transform &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;
}
Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
Transform 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);
}

262
modules/bullet/collision_object_bullet.h
View File

@ -1,262 +0,0 @@
#ifndef COLLISION_OBJECT_BULLET_H
#define COLLISION_OBJECT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/math/transform.h"
#include "core/math/vector3.h"
#include "core/object.h"
#include "core/vset.h"
#include "shape_owner_bullet.h"
#include <LinearMath/btTransform.h>
/**
@author AndreaCatania
*/
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;
btCollisionShape *bt_shape;
btTransform transform;
btVector3 scale;
bool active;
ShapeWrapper() :
shape(nullptr),
bt_shape(nullptr),
active(true) {}
ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active) :
shape(p_shape),
bt_shape(nullptr),
active(p_active) {
set_transform(p_transform);
}
ShapeWrapper(ShapeBullet *p_shape, const Transform &p_transform, bool p_active) :
shape(p_shape),
bt_shape(nullptr),
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 Transform &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;
ObjectID instance_id;
uint32_t collisionLayer;
uint32_t collisionMask;
bool collisionsEnabled;
bool m_isStatic;
bool ray_pickable;
btCollisionObject *bt_collision_object;
Vector3 body_scale;
bool force_shape_reset;
SpaceBullet *space;
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 Transform &p_global_transform);
Transform 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;
Vector<ShapeWrapper> shapes;
public:
RigidCollisionObjectBullet(Type 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 Transform &p_transform = Transform(), 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 Transform &p_transform);
const btTransform &get_bt_shape_transform(int p_index) const;
Transform 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

105
modules/bullet/cone_twist_joint_bullet.cpp
View File

@ -1,105 +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>
/**
@author AndreaCatania
*/
ConeTwistJointBullet::ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame) :
JointBullet() {
Transform scaled_AFrame(rbAFrame.scaled(rbA->get_body_scale()));
_ALLOW_DISCARD_ scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(rbBFrame.scaled(rbB->get_body_scale()));
_ALLOW_DISCARD_ 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(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
coneConstraint->setLimit(5, p_value);
coneConstraint->setLimit(4, p_value);
break;
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
coneConstraint->setLimit(3, p_value);
break;
case PhysicsServer::CONE_TWIST_JOINT_BIAS:
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), p_value, coneConstraint->getRelaxationFactor());
break;
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), p_value, coneConstraint->getBiasFactor(), coneConstraint->getRelaxationFactor());
break;
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), coneConstraint->getBiasFactor(), p_value);
break;
default:
WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
break;
}
}
real_t ConeTwistJointBullet::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
switch (p_param) {
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
return coneConstraint->getSwingSpan1();
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
return coneConstraint->getTwistSpan();
case PhysicsServer::CONE_TWIST_JOINT_BIAS:
return coneConstraint->getBiasFactor();
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
return coneConstraint->getLimitSoftness();
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
return coneConstraint->getRelaxationFactor();
default:
WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
return 0;
}
}

52
modules/bullet/cone_twist_joint_bullet.h
View File

@ -1,52 +0,0 @@
#ifndef CONE_TWIST_JOINT_BULLET_H
#define CONE_TWIST_JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "joint_bullet.h"
/**
@author AndreaCatania
*/
class RigidBodyBullet;
class ConeTwistJointBullet : public JointBullet {
class btConeTwistConstraint *coneConstraint;
public:
ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const;
};
#endif

6
modules/bullet/config.py
View File

@ -1,6 +0,0 @@
def can_build(env, platform):
return True
def configure(env):
pass

65
modules/bullet/constraint_bullet.cpp
View File

@ -1,65 +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"
/**
@author AndreaCatania
*/
ConstraintBullet::ConstraintBullet() :
space(nullptr),
constraint(nullptr),
disabled_collisions_between_bodies(true) {}
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);
}
}

70
modules/bullet/constraint_bullet.h
View File

@ -1,70 +0,0 @@
#ifndef CONSTRAINT_BULLET_H
#define CONSTRAINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "bullet_utilities.h"
#include "rid_bullet.h"
#include <BulletDynamics/ConstraintSolver/btTypedConstraint.h>
/**
@author AndreaCatania
*/
class RigidBodyBullet;
class SpaceBullet;
class btTypedConstraint;
class ConstraintBullet : public RIDBullet {
protected:
SpaceBullet *space;
btTypedConstraint *constraint;
bool disabled_collisions_between_bodies;
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

263
modules/bullet/generic_6dof_joint_bullet.cpp
View File

@ -1,263 +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>
/**
@author AndreaCatania
*/
Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB) :
JointBullet() {
Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
_ALLOW_DISCARD_ scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
_ALLOW_DISCARD_ 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);
}
Transform Generic6DOFJointBullet::getFrameOffsetA() const {
btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
Transform gTrs;
B_TO_G(btTrs, gTrs);
return gTrs;
}
Transform Generic6DOFJointBullet::getFrameOffsetB() const {
btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
Transform gTrs;
B_TO_G(btTrs, gTrs);
return gTrs;
}
Transform Generic6DOFJointBullet::getFrameOffsetA() {
btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
Transform gTrs;
B_TO_G(btTrs, gTrs);
return gTrs;
}
Transform Generic6DOFJointBullet::getFrameOffsetB() {
btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
Transform 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, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
ERR_FAIL_INDEX(p_axis, 3);
switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
limits_lower[0][p_axis] = p_value;
set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT]); // Reload bullet parameter
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
limits_upper[0][p_axis] = p_value;
set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT]); // Reload bullet parameter
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY:
sixDOFConstraint->getTranslationalLimitMotor()->m_targetVelocity.m_floats[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT:
sixDOFConstraint->getTranslationalLimitMotor()->m_maxMotorForce.m_floats[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING:
sixDOFConstraint->getTranslationalLimitMotor()->m_springDamping.m_floats[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS:
sixDOFConstraint->getTranslationalLimitMotor()->m_springStiffness.m_floats[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT:
sixDOFConstraint->getTranslationalLimitMotor()->m_equilibriumPoint.m_floats[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
limits_lower[1][p_axis] = p_value;
set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, flags[p_axis][PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]); // Reload bullet parameter
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
limits_upper[1][p_axis] = p_value;
set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, flags[p_axis][PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]); // Reload bullet parameter
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxMotorForce = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springStiffness = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springDamping = p_value;
break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_equilibriumPoint = p_value;
break;
default:
WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
break;
}
}
real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const {
ERR_FAIL_INDEX_V(p_axis, 3, 0.);
switch (p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
return limits_lower[0][p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
return limits_upper[0][p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY:
return sixDOFConstraint->getTranslationalLimitMotor()->m_targetVelocity.m_floats[p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT:
return sixDOFConstraint->getTranslationalLimitMotor()->m_maxMotorForce.m_floats[p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_DAMPING:
return sixDOFConstraint->getTranslationalLimitMotor()->m_springDamping.m_floats[p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS:
return sixDOFConstraint->getTranslationalLimitMotor()->m_springStiffness.m_floats[p_axis];
case PhysicsServer::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT:
return sixDOFConstraint->getTranslationalLimitMotor()->m_equilibriumPoint.m_floats[p_axis];
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
return limits_lower[1][p_axis];
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
return limits_upper[1][p_axis];
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxMotorForce;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springStiffness;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_DAMPING:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springDamping;
case PhysicsServer::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT:
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_equilibriumPoint;
default:
WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
return 0;
}
}
void Generic6DOFJointBullet::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
ERR_FAIL_INDEX(p_axis, 3);
flags[p_axis][p_flag] = p_value;
switch (p_flag) {
case PhysicsServer::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 PhysicsServer::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 PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = flags[p_axis][p_flag];
break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR:
sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] = flags[p_axis][p_flag];
break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING:
sixDOFConstraint->getTranslationalLimitMotor()->m_enableSpring[p_axis] = p_value;
break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING:
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableSpring = p_value;
break;
default:
WARN_DEPRECATED_MSG("The flag " + itos(p_flag) + " is deprecated.");
break;
}
}
bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
ERR_FAIL_INDEX_V(p_axis, 3, false);
return flags[p_axis][p_flag];
}

72
modules/bullet/generic_6dof_joint_bullet.h
View File

@ -1,72 +0,0 @@
#ifndef GENERIC_6DOF_JOINT_BULLET_H
#define GENERIC_6DOF_JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "joint_bullet.h"
/**
@author AndreaCatania
*/
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][PhysicsServer::G6DOF_JOINT_FLAG_MAX];
public:
Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
Transform getFrameOffsetA() const;
Transform getFrameOffsetB() const;
Transform getFrameOffsetA();
Transform 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, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value);
real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const;
void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const;
};
#endif

130
modules/bullet/godot_collision_configuration.cpp
View File

@ -1,130 +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>
/**
@author AndreaCatania
*/
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);
}
}

65
modules/bullet/godot_collision_configuration.h
View File

@ -1,65 +0,0 @@
#ifndef GODOT_COLLISION_CONFIGURATION_H
#define GODOT_COLLISION_CONFIGURATION_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
/**
@author AndreaCatania
*/
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

58
modules/bullet/godot_collision_dispatcher.cpp
View File

@ -1,58 +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"
/**
@author AndreaCatania
*/
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) {
// Avoide 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) {
// Avoide area narrow phase
return false;
}
return btCollisionDispatcher::needsResponse(body0, body1);
}

51
modules/bullet/godot_collision_dispatcher.h
View File

@ -1,51 +0,0 @@
#ifndef GODOT_COLLISION_DISPATCHER_H
#define GODOT_COLLISION_DISPATCHER_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/int_types.h"
#include <btBulletDynamicsCommon.h>
/**
@author AndreaCatania
*/
/// 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

98
modules/bullet/godot_motion_state.h
View File

@ -1,98 +0,0 @@
#ifndef GODOT_MOTION_STATE_H
#define GODOT_MOTION_STATE_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "rigid_body_bullet.h"
#include <LinearMath/btMotionState.h>
/**
@author AndreaCatania
*/
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;
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

116
modules/bullet/godot_ray_world_algorithm.cpp
View File

@ -1,116 +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>
/**
@author AndreaCatania
*/
// 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_ownManifold(false),
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;
}

83
modules/bullet/godot_ray_world_algorithm.h
View File

@ -1,83 +0,0 @@
#ifndef GODOT_RAY_WORLD_ALGORITHM_H
#define GODOT_RAY_WORLD_ALGORITHM_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include <BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h>
#include <BulletCollision/CollisionDispatch/btCollisionCreateFunc.h>
#include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
/**
@author AndreaCatania
*/
class btDiscreteDynamicsWorld;
class GodotRayWorldAlgorithm : public btActivatingCollisionAlgorithm {
const btDiscreteDynamicsWorld *m_world;
btPersistentManifold *m_manifoldPtr;
bool m_ownManifold;
bool m_isSwapped;
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

391
modules/bullet/godot_result_callbacks.cpp
View File

@ -1,391 +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>
/**
@author AndreaCatania
*/
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::test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask) {
return body0_collision_layer & body1_collision_mask || body1_collision_layer & body0_collision_mask;
}
bool GodotFilterCallback::needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const {
return GodotFilterCallback::test_collision_filters(proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask, proxy1->m_collisionFilterGroup, proxy1->m_collisionFilterMask);
}
bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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;
}
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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());
PhysicsDirectSpaceState::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 = 0 == result.collider_id ? nullptr : ObjectDB::get_instance(result.collider_id);
++count;
return 1; // not used by bullet
}
bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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 {
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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;
}
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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) {
PhysicsDirectSpaceState::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 = 0 == result.collider_id ? 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;
}
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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 {
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
if (needs) {
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;
}
}

239
modules/bullet/godot_result_callbacks.h
View File

@ -1,239 +0,0 @@
#ifndef GODOT_RESULT_CALLBACKS_H
#define GODOT_RESULT_CALLBACKS_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "servers/physics_server.h"
#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
#include <btBulletDynamicsCommon.h>
/**
@author AndreaCatania
*/
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 {
static bool test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask);
// 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;
int m_shapeId;
bool collide_with_bodies;
bool collide_with_areas;
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),
m_pickRay(false),
m_shapeId(0),
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:
PhysicsDirectSpaceState::ShapeResult *m_results;
int m_resultMax;
const Set<RID> *m_exclude;
int count;
GodotAllConvexResultCallback(PhysicsDirectSpaceState::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude) :
m_results(p_results),
m_resultMax(p_resultMax),
m_exclude(p_exclude),
count(0) {}
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;
bool collide_with_bodies;
bool collide_with_areas;
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),
m_shapeId(0),
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;
PhysicsDirectSpaceState::ShapeResult *m_results;
int m_resultMax;
const Set<RID> *m_exclude;
int m_count;
bool collide_with_bodies;
bool collide_with_areas;
GodotAllContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::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),
m_count(0),
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;
int m_resultMax;
const Set<RID> *m_exclude;
int m_count;
bool collide_with_bodies;
bool collide_with_areas;
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),
m_count(0),
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;
PhysicsDirectSpaceState::ShapeRestInfo *m_result;
const Set<RID> *m_exclude;
bool m_collided;
real_t m_min_distance;
const btCollisionObject *m_rest_info_collision_object;
btVector3 m_rest_info_bt_point;
bool collide_with_bodies;
bool collide_with_areas;
GodotRestInfoContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::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),
m_collided(false),
m_min_distance(0),
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;
int m_other_compound_shape_index;
GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) :
btManifoldResult(body0Wrap, body1Wrap),
m_penetration_distance(0),
m_other_compound_shape_index(0) {}
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

169
modules/bullet/hinge_joint_bullet.cpp
View File

@ -1,169 +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>
/**
@author AndreaCatania
*/
HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB) :
JointBullet() {
Transform scaled_AFrame(frameA.scaled(rbA->get_body_scale()));
_ALLOW_DISCARD_ scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameB.scaled(rbB->get_body_scale()));
_ALLOW_DISCARD_ 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(PhysicsServer::HingeJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), p_value, hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
break;
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
hingeConstraint->setLimit(p_value, hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
break;
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), p_value, hingeConstraint->getLimitRelaxationFactor());
break;
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), p_value, hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
break;
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), p_value);
break;
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
hingeConstraint->setMotorTargetVelocity(p_value);
break;
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
hingeConstraint->setMaxMotorImpulse(p_value);
break;
default:
WARN_DEPRECATED_MSG("The HingeJoint parameter " + itos(p_param) + " is deprecated.");
break;
}
}
real_t HingeJointBullet::get_param(PhysicsServer::HingeJointParam p_param) const {
switch (p_param) {
case PhysicsServer::HINGE_JOINT_BIAS:
return 0;
break;
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
return hingeConstraint->getUpperLimit();
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
return hingeConstraint->getLowerLimit();
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
return hingeConstraint->getLimitBiasFactor();
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
return hingeConstraint->getLimitSoftness();
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
return hingeConstraint->getLimitRelaxationFactor();
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
return hingeConstraint->getMotorTargetVelocity();
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
return hingeConstraint->getMaxMotorImpulse();
default:
WARN_DEPRECATED_MSG("The HingeJoint parameter " + itos(p_param) + " is deprecated.");
return 0;
}
}
void HingeJointBullet::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) {
switch (p_flag) {
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
if (!p_value) {
hingeConstraint->setLimit(-Math_PI, Math_PI);
}
break;
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
hingeConstraint->enableMotor(p_value);
break;
case PhysicsServer::HINGE_JOINT_FLAG_MAX:
break; // Can't happen, but silences warning
}
}
bool HingeJointBullet::get_flag(PhysicsServer::HingeJointFlag p_flag) const {
switch (p_flag) {
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
return true;
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
return hingeConstraint->getEnableAngularMotor();
default:
return false;
}
}

56
modules/bullet/hinge_joint_bullet.h
View File

@ -1,56 +0,0 @@
#ifndef HINGE_JOINT_BULLET_H
#define HINGE_JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "joint_bullet.h"
/**
@author AndreaCatania
*/
class HingeJointBullet : public JointBullet {
class btHingeConstraint *hingeConstraint;
public:
HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB);
HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
real_t get_hinge_angle();
void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer::HingeJointParam p_param) const;
void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
};
#endif

42
modules/bullet/joint_bullet.cpp
View File

@ -1,42 +0,0 @@
/*************************************************************************/
/* 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 "joint_bullet.h"
#include "space_bullet.h"
/**
@author AndreaCatania
*/
JointBullet::JointBullet() :
ConstraintBullet() {}
JointBullet::~JointBullet() {}

50
modules/bullet/joint_bullet.h
View File

@ -1,50 +0,0 @@
#ifndef JOINT_BULLET_H
#define JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "constraint_bullet.h"
#include "servers/physics_server.h"
/**
@author AndreaCatania
*/
class RigidBodyBullet;
class btTypedConstraint;
class JointBullet : public ConstraintBullet {
public:
JointBullet();
virtual ~JointBullet();
virtual PhysicsServer::JointType get_type() const = 0;
};
#endif

116
modules/bullet/pin_joint_bullet.cpp
View File

@ -1,116 +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>
/**
@author AndreaCatania
*/
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(PhysicsServer::PinJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer::PIN_JOINT_BIAS:
p2pConstraint->m_setting.m_tau = p_value;
break;
case PhysicsServer::PIN_JOINT_DAMPING:
p2pConstraint->m_setting.m_damping = p_value;
break;
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
p2pConstraint->m_setting.m_impulseClamp = p_value;
break;
}
}
real_t PinJointBullet::get_param(PhysicsServer::PinJointParam p_param) const {
switch (p_param) {
case PhysicsServer::PIN_JOINT_BIAS:
return p2pConstraint->m_setting.m_tau;
case PhysicsServer::PIN_JOINT_DAMPING:
return p2pConstraint->m_setting.m_damping;
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
return p2pConstraint->m_setting.m_impulseClamp;
default:
WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
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;
}

59
modules/bullet/pin_joint_bullet.h
View File

@ -1,59 +0,0 @@
#ifndef PIN_JOINT_BULLET_H
#define PIN_JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "joint_bullet.h"
/**
@author AndreaCatania
*/
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 PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
void set_param(PhysicsServer::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer::PinJointParam p_param) const;
void setPivotInA(const Vector3 &p_pos);
void setPivotInB(const Vector3 &p_pos);
Vector3 getPivotInA();
Vector3 getPivotInB();
};
#endif

58
modules/bullet/register_types.cpp
View File

@ -1,58 +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/class_db.h"
#include "core/project_settings.h"
/**
@author AndreaCatania
*/
#ifndef _3D_DISABLED
PhysicsServer *_createBulletPhysicsCallback() {
return memnew(BulletPhysicsServer);
}
#endif
void register_bullet_types() {
#ifndef _3D_DISABLED
PhysicsServerManager::register_server("Bullet", &_createBulletPhysicsCallback);
PhysicsServerManager::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() {
}

39
modules/bullet/register_types.h
View File

@ -1,39 +0,0 @@
#ifndef REGISTER_BULLET_TYPES_H
#define REGISTER_BULLET_TYPES_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
/**
@author AndreaCatania
*/
void register_bullet_types();
void unregister_bullet_types();
#endif

52
modules/bullet/rid_bullet.h
View File

@ -1,52 +0,0 @@
#ifndef RID_BULLET_H
#define RID_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/rid.h"
/**
@author AndreaCatania
*/
class BulletPhysicsServer;
class RIDBullet : public RID_Data {
RID self;
BulletPhysicsServer *physicsServer;
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(BulletPhysicsServer *p_physicsServer) { physicsServer = p_physicsServer; }
_FORCE_INLINE_ BulletPhysicsServer *get_physics_server() const { return physicsServer; }
};
#endif

1078
modules/bullet/rigid_body_bullet.cpp
View File

File diff suppressed because it is too large Load Diff

302
modules/bullet/rigid_body_bullet.h
View File

@ -1,302 +0,0 @@
#ifndef BODYBULLET_H
#define BODYBULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "collision_object_bullet.h"
#include "space_bullet.h"
#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h>
#include <LinearMath/btTransform.h>
/**
@author AndreaCatania
*/
class AreaBullet;
class SpaceBullet;
class btRigidBody;
class GodotMotionState;
class BulletPhysicsDirectBodyState : public PhysicsDirectBodyState {
GDCLASS(BulletPhysicsDirectBodyState, PhysicsDirectBodyState);
public:
RigidBodyBullet *body = nullptr;
BulletPhysicsDirectBodyState() {}
virtual Vector3 get_total_gravity() const;
virtual float get_total_angular_damp() const;
virtual float get_total_linear_damp() const;
virtual Vector3 get_center_of_mass() const;
virtual Basis get_principal_inertia_axes() const;
// get the mass
virtual float get_inverse_mass() const;
// get density of this body space
virtual Vector3 get_inverse_inertia() const;
// get density of this body space
virtual Basis get_inverse_inertia_tensor() const;
virtual void set_linear_velocity(const Vector3 &p_velocity);
virtual Vector3 get_linear_velocity() const;
virtual void set_angular_velocity(const Vector3 &p_velocity);
virtual Vector3 get_angular_velocity() const;
virtual void set_transform(const Transform &p_transform);
virtual Transform get_transform() const;
virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const;
virtual void add_central_force(const Vector3 &p_force);
virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos);
virtual void add_torque(const Vector3 &p_torque);
virtual void apply_central_impulse(const Vector3 &p_impulse);
virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse);
virtual void apply_torque_impulse(const Vector3 &p_impulse);
virtual void set_sleep_state(bool p_enable);
virtual bool is_sleeping() const;
virtual int get_contact_count() const;
virtual Vector3 get_contact_local_position(int p_contact_idx) const;
virtual Vector3 get_contact_local_normal(int p_contact_idx) const;
virtual float get_contact_impulse(int p_contact_idx) const;
virtual int get_contact_local_shape(int p_contact_idx) const;
virtual RID get_contact_collider(int p_contact_idx) const;
virtual Vector3 get_contact_collider_position(int p_contact_idx) const;
virtual ObjectID get_contact_collider_id(int p_contact_idx) const;
virtual int get_contact_collider_shape(int p_contact_idx) const;
virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const;
virtual real_t get_step() const;
virtual void integrate_forces() {
// Skip the execution of this function
}
virtual PhysicsDirectSpaceState *get_space_state();
};
class RigidBodyBullet : public RigidCollisionObjectBullet {
public:
struct CollisionData {
RigidBodyBullet *otherObject;
int other_object_shape;
int local_shape;
Vector3 hitLocalLocation;
Vector3 hitWorldLocation;
Vector3 hitNormal;
float appliedImpulse;
};
struct ForceIntegrationCallback {
ObjectID id;
StringName method;
Variant udata;
};
/// Used to hold shapes
struct KinematicShape {
class btConvexShape *shape;
btTransform transform;
KinematicShape() :
shape(nullptr) {}
bool is_active() const { return shape; }
};
struct KinematicUtilities {
RigidBodyBullet *owner;
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:
BulletPhysicsDirectBodyState *direct_access = nullptr;
friend class BulletPhysicsDirectBodyState;
// This is required only for Kinematic movement
KinematicUtilities *kinematic_utilities;
PhysicsServer::BodyMode mode;
GodotMotionState *godotMotionState;
btRigidBody *btBody;
uint16_t locked_axis;
real_t mass;
real_t gravity_scale;
real_t linearDamp;
real_t angularDamp;
bool can_sleep;
bool omit_forces_integration;
bool can_integrate_forces;
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;
int collisionsCount;
int prev_collision_count;
Vector<AreaBullet *> areasWhereIam;
// these parameters are used to avoid vector resize
int maxAreasWhereIam;
int areaWhereIamCount;
// Used to know if the area is used as gravity point
int countGravityPointSpaces;
bool isScratchedSpaceOverrideModificator;
bool previousActiveState; // Last check state
ForceIntegrationCallback *force_integration_callback;
public:
RigidBodyBullet();
~RigidBodyBullet();
BulletPhysicsDirectBodyState *get_direct_state() const { return direct_access; }
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(ObjectID p_id, const StringName &p_method, 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 float &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(PhysicsServer::BodyParameter p_param, real_t);
real_t get_param(PhysicsServer::BodyParameter p_param) const;
void set_mode(PhysicsServer::BodyMode p_mode);
PhysicsServer::BodyMode get_mode() const;
void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant);
Variant get_state(PhysicsServer::BodyState p_state) const;
void apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse);
void apply_central_impulse(const Vector3 &p_impulse);
void apply_torque_impulse(const Vector3 &p_impulse);
void apply_force(const Vector3 &p_force, const Vector3 &p_pos);
void apply_central_force(const Vector3 &p_force);
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(PhysicsServer::BodyAxis p_axis, bool lock);
bool is_axis_locked(PhysicsServer::BodyAxis p_axis) const;
void reload_axis_lock();
/// Doc:
/// https://web.archive.org/web/20180404091446/http://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

600
modules/bullet/shape_bullet.cpp
View File

@ -1,600 +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/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>
/**
@author AndreaCatania
*/
ShapeBullet::ShapeBullet() :
margin(0.04) {}
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 (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
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_plane(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));
}
btCapsuleShapeZ *ShapeBullet::create_shape_capsule(btScalar radius, btScalar height) {
return bulletnew(btCapsuleShapeZ(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(PoolVector<real_t> &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.read().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 PhysicsServer.shape_create(PhysicsServer.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;
}
/* PLANE */
PlaneShapeBullet::PlaneShapeBullet() :
ShapeBullet() {}
void PlaneShapeBullet::set_data(const Variant &p_data) {
setup(p_data);
}
Variant PlaneShapeBullet::get_data() const {
return plane;
}
PhysicsServer::ShapeType PlaneShapeBullet::get_type() const {
return PhysicsServer::SHAPE_PLANE;
}
void PlaneShapeBullet::setup(const Plane &p_plane) {
plane = p_plane;
notifyShapeChanged();
}
btCollisionShape *PlaneShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
btVector3 btPlaneNormal;
G_TO_B(plane.normal, btPlaneNormal);
return prepare(PlaneShapeBullet::create_shape_plane(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;
}
PhysicsServer::ShapeType SphereShapeBullet::get_type() const {
return PhysicsServer::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;
}
PhysicsServer::ShapeType BoxShapeBullet::get_type() const {
return PhysicsServer::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;
}
PhysicsServer::ShapeType CapsuleShapeBullet::get_type() const {
return PhysicsServer::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] + p_extra_edge));
}
/* 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;
}
PhysicsServer::ShapeType CylinderShapeBullet::get_type() const {
return PhysicsServer::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;
}
PhysicsServer::ShapeType ConvexPolygonShapeBullet::get_type() const {
return PhysicsServer::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(),
meshShape(nullptr) {}
ConcavePolygonShapeBullet::~ConcavePolygonShapeBullet() {
if (meshShape) {
delete meshShape->getMeshInterface();
delete meshShape->getTriangleInfoMap();
bulletdelete(meshShape);
}
faces = PoolVector<Vector3>();
}
void ConcavePolygonShapeBullet::set_data(const Variant &p_data) {
setup(p_data);
}
Variant ConcavePolygonShapeBullet::get_data() const {
return faces;
}
PhysicsServer::ShapeType ConcavePolygonShapeBullet::get_type() const {
return PhysicsServer::SHAPE_CONCAVE_POLYGON;
}
void ConcavePolygonShapeBullet::setup(PoolVector<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;
PoolVector<Vector3>::Read r = p_faces.read();
const Vector3 *facesr = r.ptr();
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.");
// TODO This code will need adjustments if real_t is set to `double`,
// because that precision is unnecessary for a heightmap and Bullet doesn't support it...
PoolVector<real_t> l_heights;
Variant l_heights_v = d["heights"];
if (l_heights_v.get_type() == Variant::POOL_REAL_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);
PoolByteArray im_data = l_image->get_data();
l_heights.resize(l_image->get_width() * l_image->get_height());
PoolRealArray::Write w = l_heights.write();
PoolByteArray::Read r = im_data.read();
float *rp = (float *)r.ptr();
// 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 PoolRealArray 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")) {
PoolVector<real_t>::Read r = l_heights.read();
int heights_size = l_heights.size();
for (int i = 0; i < heights_size; ++i) {
real_t 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());
}
PhysicsServer::ShapeType HeightMapShapeBullet::get_type() const {
return PhysicsServer::SHAPE_HEIGHTMAP;
}
void HeightMapShapeBullet::setup(PoolVector<real_t> &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(),
length(1),
slips_on_slope(false) {}
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;
}
PhysicsServer::ShapeType RayShapeBullet::get_type() const {
return PhysicsServer::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));
}

246
modules/bullet/shape_bullet.h
View File

@ -1,246 +0,0 @@
#ifndef SHAPE_BULLET_H
#define SHAPE_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/math/geometry.h"
#include "core/variant.h"
#include "rid_bullet.h"
#include "servers/physics_server.h"
#include <LinearMath/btAlignedObjectArray.h>
#include <LinearMath/btScalar.h>
#include <LinearMath/btVector3.h>
/**
@author AndreaCatania
*/
class ShapeBullet;
class btCollisionShape;
class ShapeOwnerBullet;
class btBvhTriangleMeshShape;
class ShapeBullet : public RIDBullet {
Map<ShapeOwnerBullet *, int> owners;
real_t margin;
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 PhysicsServer::ShapeType get_type() const = 0;
public:
static class btEmptyShape *create_shape_empty();
static class btStaticPlaneShape *create_shape_plane(const btVector3 &planeNormal, btScalar planeConstant);
static class btSphereShape *create_shape_sphere(btScalar radius);
static class btBoxShape *create_shape_box(const btVector3 &boxHalfExtents);
static class btCapsuleShapeZ *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(PoolVector<real_t> &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 PlaneShapeBullet : public ShapeBullet {
Plane plane;
public:
PlaneShapeBullet();
virtual void set_data(const Variant &p_data);
virtual Variant get_data() const;
virtual PhysicsServer::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;
public:
SphereShapeBullet();
_FORCE_INLINE_ real_t get_radius() { return radius; }
virtual void set_data(const Variant &p_data);
virtual Variant get_data() const;
virtual PhysicsServer::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 PhysicsServer::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;
real_t radius;
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 PhysicsServer::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;
real_t radius;
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 PhysicsServer::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 PhysicsServer::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;
public:
PoolVector<Vector3> faces;
ConcavePolygonShapeBullet();
virtual ~ConcavePolygonShapeBullet();
virtual void set_data(const Variant &p_data);
virtual Variant get_data() const;
virtual PhysicsServer::ShapeType get_type() const;
virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
private:
void setup(PoolVector<Vector3> p_faces);
};
class HeightMapShapeBullet : public ShapeBullet {
public:
PoolVector<real_t> heights;
int width;
int depth;
real_t min_height;
real_t max_height;
HeightMapShapeBullet();
virtual void set_data(const Variant &p_data);
virtual Variant get_data() const;
virtual PhysicsServer::ShapeType get_type() const;
virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
private:
void setup(PoolVector<real_t> &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;
bool slips_on_slope;
RayShapeBullet();
virtual void set_data(const Variant &p_data);
virtual Variant get_data() const;
virtual PhysicsServer::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

35
modules/bullet/shape_owner_bullet.cpp
View File

@ -1,35 +0,0 @@
/*************************************************************************/
/* shape_owner_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_owner_bullet.h"
/**
@author AndreaCatania
*/

53
modules/bullet/shape_owner_bullet.h
View File

@ -1,53 +0,0 @@
#ifndef SHAPE_OWNER_BULLET_H
#define SHAPE_OWNER_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "rid_bullet.h"
/**
@author AndreaCatania
*/
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

465
modules/bullet/slider_joint_bullet.cpp
View File

@ -1,465 +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>
/**
@author AndreaCatania
*/
SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB) :
JointBullet() {
Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
_ALLOW_DISCARD_ scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
_ALLOW_DISCARD_ 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 Transform SliderJointBullet::getCalculatedTransformA() const {
btTransform btTransform = sliderConstraint->getCalculatedTransformA();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
const Transform SliderJointBullet::getCalculatedTransformB() const {
btTransform btTransform = sliderConstraint->getCalculatedTransformB();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
const Transform SliderJointBullet::getFrameOffsetA() const {
btTransform btTransform = sliderConstraint->getFrameOffsetA();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
const Transform SliderJointBullet::getFrameOffsetB() const {
btTransform btTransform = sliderConstraint->getFrameOffsetB();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
Transform SliderJointBullet::getFrameOffsetA() {
btTransform btTransform = sliderConstraint->getFrameOffsetA();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
Transform SliderJointBullet::getFrameOffsetB() {
btTransform btTransform = sliderConstraint->getFrameOffsetB();
Transform 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(PhysicsServer::SliderJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
setUpperLinLimit(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
setLowerLinLimit(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
setSoftnessLimLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
setRestitutionLimLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
setDampingLimLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
setSoftnessDirLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
setRestitutionDirLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
setDampingDirLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
setSoftnessOrthoLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
setRestitutionOrthoLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
setDampingOrthoLin(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
setUpperAngLimit(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
setLowerAngLimit(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
setSoftnessLimAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
setRestitutionLimAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
setDampingLimAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
setSoftnessDirAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
setRestitutionDirAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
setDampingDirAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
setSoftnessOrthoAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
setRestitutionOrthoAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
setDampingOrthoAng(p_value);
break;
case PhysicsServer::SLIDER_JOINT_MAX:
break; // Can't happen, but silences warning
}
}
real_t SliderJointBullet::get_param(PhysicsServer::SliderJointParam p_param) const {
switch (p_param) {
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
return getUpperLinLimit();
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
return getLowerLinLimit();
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
return getSoftnessLimLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
return getRestitutionLimLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
return getDampingLimLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
return getSoftnessDirLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
return getRestitutionDirLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
return getDampingDirLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
return getSoftnessOrthoLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
return getRestitutionOrthoLin();
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
return getDampingOrthoLin();
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
return getUpperAngLimit();
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
return getLowerAngLimit();
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
return getSoftnessLimAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
return getRestitutionLimAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
return getDampingLimAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
return getSoftnessDirAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
return getRestitutionDirAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
return getDampingDirAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
return getSoftnessOrthoAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
return getRestitutionOrthoAng();
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
return getDampingOrthoAng();
default:
return 0;
}
}

120
modules/bullet/slider_joint_bullet.h
View File

@ -1,120 +0,0 @@
#ifndef SLIDER_JOINT_BULLET_H
#define SLIDER_JOINT_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "joint_bullet.h"
/**
@author AndreaCatania
*/
class RigidBodyBullet;
class SliderJointBullet : public JointBullet {
class btSliderConstraint *sliderConstraint;
public:
/// Reference frame is A
SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB);
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
const RigidBodyBullet *getRigidBodyA() const;
const RigidBodyBullet *getRigidBodyB() const;
const Transform getCalculatedTransformA() const;
const Transform getCalculatedTransformB() const;
const Transform getFrameOffsetA() const;
const Transform getFrameOffsetB() const;
Transform getFrameOffsetA();
Transform 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(PhysicsServer::SliderJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer::SliderJointParam p_param) const;
};
#endif

490
modules/bullet/soft_body_bullet.cpp
View File

@ -1,490 +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 "scene/3d/soft_body.h"
#include "space_bullet.h"
SoftBodyBullet::SoftBodyBullet() :
CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY),
bt_soft_body(nullptr),
isScratched(false),
simulation_precision(5),
total_mass(1.),
linear_stiffness(0.5),
areaAngular_stiffness(0.5),
volume_stiffness(0.5),
pressure_coefficient(0.),
pose_matching_coefficient(0.),
damping_coefficient(0.01),
drag_coefficient(0.) {}
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_visual_server(SoftBodyVisualServerHandler *p_visual_server_handler) {
if (!bt_soft_body) {
return;
}
/// Update visual 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_visual_server_handler->set_vertex((*vs_indices)[x], vertex_position);
p_visual_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_visual_server_handler->set_aabb(aabb);
}
void SoftBodyBullet::set_soft_mesh(const Ref<Mesh> &p_mesh) {
destroy_soft_body();
soft_mesh = p_mesh;
if (soft_mesh.is_null()) {
return;
}
ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & VS::ARRAY_FORMAT_INDEX));
Array arrays = soft_mesh->surface_get_arrays(0);
set_trimesh_body_shape(arrays[VS::ARRAY_INDEX], arrays[VS::ARRAY_VERTEX]);
}
void SoftBodyBullet::destroy_soft_body() {
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 Transform &p_transform) {
reset_all_node_positions();
move_all_nodes(p_transform);
}
void SoftBodyBullet::move_all_nodes(const Transform &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) {
ERR_FAIL_INDEX(p_node_index, bt_soft_body->m_nodes.size());
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) {
ERR_FAIL_INDEX(p_node_index, bt_soft_body->m_nodes.size());
B_TO_G(bt_soft_body->m_nodes[p_node_index].m_x, r_position);
}
}
void SoftBodyBullet::get_node_offset(int p_node_index, Vector3 &r_offset) const {
if (soft_mesh.is_null()) {
return;
}
Array arrays = soft_mesh->surface_get_arrays(0);
PoolVector<Vector3> vertices(arrays[VS::ARRAY_VERTEX]);
if (0 <= p_node_index && vertices.size() > p_node_index) {
r_offset = vertices[p_node_index];
}
}
void SoftBodyBullet::get_node_offset(int p_node_index, btVector3 &r_offset) const {
Vector3 off;
get_node_offset(p_node_index, off);
G_TO_B(off, r_offset);
}
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() || !bt_soft_body) {
return;
}
Array arrays = soft_mesh->surface_get_arrays(0);
PoolVector<Vector3> vs_vertices(arrays[VS::ARRAY_VERTEX]);
PoolVector<Vector3>::Read vs_vertices_read = vs_vertices.read();
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 (!bt_soft_body) {
return;
}
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_areaAngular_stiffness(real_t p_val) {
areaAngular_stiffness = p_val;
if (bt_soft_body) {
mat0->m_kAST = areaAngular_stiffness;
}
}
void SoftBodyBullet::set_volume_stiffness(real_t p_val) {
volume_stiffness = p_val;
if (bt_soft_body) {
mat0->m_kVST = volume_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_pose_matching_coefficient(real_t p_val) {
pose_matching_coefficient = p_val;
if (bt_soft_body) {
bt_soft_body->m_cfg.kMT = pose_matching_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;
}
}
void SoftBodyBullet::set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices) {
/// Assert the current soft body is destroyed
destroy_soft_body();
/// Parse visual server indices to physical indices.
/// Merge all overlapping vertices and create a map of physical vertices to visual server
{
/// This is the map of visual 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());
PoolVector<Vector3>::Read p_vertices_read = p_vertices.read();
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 rxisting
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);
PoolVector<Vector3>::Read p_vertices_read = p_vertices.read();
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);
PoolVector<int>::Read p_indices_read = p_indices.read();
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();
}
}
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;
mat0->m_kAST = areaAngular_stiffness;
mat0->m_kVST = volume_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->m_cfg.kMT = pose_matching_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(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;
}

161
modules/bullet/soft_body_bullet.h
View File

@ -1,161 +0,0 @@
#ifndef SOFT_BODY_BULLET_H
#define SOFT_BODY_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "collision_object_bullet.h"
#include "scene/resources/material.h" // TODO remove this please
#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 "scene/resources/mesh.h"
#include "servers/physics_server.h"
#ifdef x11_None
/// This is required to re add the macro None defined by x11 compiler
#undef x11_None
#define None 0L
#endif
/**
@author AndreaCatania
*/
class SoftBodyBullet : public CollisionObjectBullet {
private:
btSoftBody *bt_soft_body;
Vector<Vector<int>> indices_table;
btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody
bool isScratched;
Ref<Mesh> soft_mesh;
int simulation_precision;
real_t total_mass;
real_t linear_stiffness; // [0,1]
real_t areaAngular_stiffness; // [0,1]
real_t volume_stiffness; // [0,1]
real_t pressure_coefficient; // [-inf,+inf]
real_t pose_matching_coefficient; // [0,1]
real_t damping_coefficient; // [0,1]
real_t drag_coefficient; // [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_visual_server(class SoftBodyVisualServerHandler *p_visual_server_handler);
void set_soft_mesh(const Ref<Mesh> &p_mesh);
void destroy_soft_body();
// Special function. This function has bad performance
void set_soft_transform(const Transform &p_transform);
void move_all_nodes(const Transform &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;
// Heavy function, Please cache this info
void get_node_offset(int node_index, Vector3 &r_offset) const;
// Heavy function, Please cache this info
void get_node_offset(int node_index, btVector3 &r_offset) 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_areaAngular_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_areaAngular_stiffness() const { return areaAngular_stiffness; }
void set_volume_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_volume_stiffness() const { return volume_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_pose_matching_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_pose_matching_coefficient() const { return pose_matching_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:
void set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<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

1466
modules/bullet/space_bullet.cpp
View File

File diff suppressed because it is too large Load Diff

226
modules/bullet/space_bullet.h
View File

@ -1,226 +0,0 @@
#ifndef SPACE_BULLET_H
#define SPACE_BULLET_H
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "core/variant.h"
#include "core/vector.h"
#include "godot_result_callbacks.h"
#include "rid_bullet.h"
#include "servers/physics_server.h"
#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
#include <BulletCollision/BroadphaseCollision/btOverlappingPairCache.h>
#include <LinearMath/btScalar.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btVector3.h>
/**
@author AndreaCatania
*/
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 PhysicsDirectSpaceState {
GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState);
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 = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false);
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 = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false);
virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false);
virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &r_closest_safe, float &r_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr);
/// Returns the list of contacts pairs in this order: Local contact, other body contact
virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float 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 = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false);
virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false);
virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const;
};
class SpaceBullet : public RIDBullet {
friend class AreaBullet;
friend void onBulletTickCallback(btDynamicsWorld *world, btScalar timeStep);
friend class BulletPhysicsDirectSpaceState;
btBroadphaseInterface *broadphase;
btDefaultCollisionConfiguration *collisionConfiguration;
btCollisionDispatcher *dispatcher;
btConstraintSolver *solver;
btDiscreteDynamicsWorld *dynamicsWorld;
btSoftBodyWorldInfo *soft_body_world_info;
btGhostPairCallback *ghostPairCallback;
GodotFilterCallback *godotFilterCallback;
btGjkEpaPenetrationDepthSolver *gjk_epa_pen_solver;
btVoronoiSimplexSolver *gjk_simplex_solver;
BulletPhysicsDirectSpaceState *direct_access;
Vector3 gravityDirection;
real_t gravityMagnitude;
real_t linear_damp;
real_t angular_damp;
Vector<AreaBullet *> areas;
Vector<Vector3> contactDebug;
int contactDebugCount;
real_t delta_time;
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() { return broadphase; }
_FORCE_INLINE_ btCollisionDispatcher *get_dispatcher() { return dispatcher; }
_FORCE_INLINE_ btSoftBodyWorldInfo *get_soft_body_world_info() { 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(PhysicsServer::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(PhysicsServer::AreaParameter p_param);
void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer::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.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 Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude = Set<RID>());
int test_ray_separation(RigidBodyBullet *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, float 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;
btVector3 normal;
btVector3 pointWorld;
btScalar penetration_distance; // Negative mean penetration
int other_compound_shape_index;
const btCollisionObject *other_collision_object;
int local_shape_most_recovered;
RecoverResult() :
hasPenetration(false),
normal(0, 0, 0),
pointWorld(0, 0, 0),
penetration_distance(1e20),
other_compound_shape_index(0),
other_collision_object(nullptr),
local_shape_most_recovered(0) {}
};
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(PhysicsServer::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, PhysicsServer::SeparationResult *r_results);
};
#endif

17
platform/server/detect.py
View File

@ -153,23 +153,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")

17
platform/x11/detect.py
View File

@ -250,23 +250,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")

40
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h vendored
View File

@ -1,40 +0,0 @@
#ifndef B3_BROADPHASE_CALLBACK_H
#define B3_BROADPHASE_CALLBACK_H
/*
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 "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

1352
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp vendored
View File

File diff suppressed because it is too large Load Diff

1334
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h vendored
View File

File diff suppressed because it is too large Load Diff

808
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp vendored
View File

@ -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

199
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h vendored
View File

@ -1,199 +0,0 @@
#ifndef B3_DBVT_BROADPHASE_H
#define B3_DBVT_BROADPHASE_H
/*
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 "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

72
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h vendored
View File

@ -1,72 +0,0 @@
#ifndef B3_OVERLAPPING_PAIR_H
#define B3_OVERLAPPING_PAIR_H
/*
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 "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

559
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp vendored
View File

@ -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
}

429
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h vendored
View File

@ -1,429 +0,0 @@
#ifndef B3_OVERLAPPING_PAIR_CACHE_H
#define B3_OVERLAPPING_PAIR_CACHE_H
/*
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 "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

56
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h vendored
View File

@ -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

39
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h vendored
View File

@ -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

57
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h vendored
View File

@ -1,57 +0,0 @@
#ifndef B3_CONTACT4_H
#define B3_CONTACT4_H
/*
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 "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

500
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp vendored
View File

@ -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
}

57
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h vendored
View File

@ -1,57 +0,0 @@
#ifndef _BT_CONVEX_UTILITY_H
#define _BT_CONVEX_UTILITY_H
/*
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 "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

297
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp vendored
View File

@ -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];
}

92
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h vendored
View File

@ -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

25
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h vendored
View File

@ -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

30
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h vendored
View File

@ -1,30 +0,0 @@
#ifndef B3_RIGID_BODY_CL
#define B3_RIGID_BODY_CL
/*
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 "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

19
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h vendored
View File

@ -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

123
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h vendored
View File

@ -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;
}
}
}
}
}
}

171
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h vendored
View File

@ -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

69
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h vendored
View File

@ -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

36
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h vendored
View File

@ -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

486
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h vendored
View File

@ -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

153
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h vendored
View File

@ -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

38
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h vendored
View File

@ -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

797
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h vendored
View File

@ -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

197
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h vendored
View File

@ -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

890
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h vendored
View File

@ -1,890 +0,0 @@
#ifndef B3_MPR_PENETRATION_H
#define B3_MPR_PENETRATION_H
/***
* ---------------------------------
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
*
* 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.
*/
#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

175
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h vendored
View File

@ -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

88
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h vendored
View File

@ -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

89
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h vendored
View File

@ -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

31
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h vendored
View File

@ -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

35
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h vendored
View File

@ -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

186
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.cpp vendored
View File

@ -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

112
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.h vendored
View File

@ -1,112 +0,0 @@
#ifndef B3_ALIGNED_ALLOCATOR
#define B3_ALIGNED_ALLOCATOR
/*
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.
*/
///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

524
thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h vendored
View File

@ -1,524 +0,0 @@
#ifndef B3_OBJECT_ARRAY__
#define B3_OBJECT_ARRAY__
/*
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 "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: https://bulletphysics.orgphpBB2/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__

106
thirdparty/bullet/Bullet3Common/b3CommandLineArgs.h vendored
View File

@ -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

133
thirdparty/bullet/Bullet3Common/b3FileUtils.h vendored
View File

@ -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

464
thirdparty/bullet/Bullet3Common/b3HashMap.h vendored
View File

@ -1,464 +0,0 @@
#ifndef B3_HASH_MAP_H
#define B3_HASH_MAP_H
/*
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 "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

145
thirdparty/bullet/Bullet3Common/b3Logging.cpp vendored
View File

@ -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

74
thirdparty/bullet/Bullet3Common/b3Logging.h vendored
View File

@ -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

1356
thirdparty/bullet/Bullet3Common/b3Matrix3x3.h vendored
View File

File diff suppressed because it is too large Load Diff

71
thirdparty/bullet/Bullet3Common/b3MinMax.h vendored
View File

@ -1,71 +0,0 @@
#ifndef B3_GEN_MINMAX_H
#define B3_GEN_MINMAX_H
/*
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.
*/
#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

123
thirdparty/bullet/Bullet3Common/b3PoolAllocator.h vendored
View File

@ -1,123 +0,0 @@
#ifndef _BT_POOL_ALLOCATOR_H
#define _BT_POOL_ALLOCATOR_H
/*
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.
*/
#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

244
thirdparty/bullet/Bullet3Common/b3QuadWord.h vendored
View File

@ -1,244 +0,0 @@
#ifndef B3_SIMD_QUADWORD_H
#define B3_SIMD_QUADWORD_H
/*
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.
*/
#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

910
thirdparty/bullet/Bullet3Common/b3Quaternion.h vendored
View File

@ -1,910 +0,0 @@
#ifndef B3_SIMD__QUATERNION_H_
#define B3_SIMD__QUATERNION_H_
/*
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.
*/
#include "b3Vector3.h"
#include "b3QuadWord.h"
#ifdef B3_USE_SSE
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
#endif
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
#endif
/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
class b3Quaternion : public b3QuadWord
{
public:
/**@brief No initialization constructor */
b3Quaternion() {}
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
// Set Vector
B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
{
mVec128 = vec;
}
// Copy constructor
B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
B3_FORCE_INLINE b3Quaternion&
operator=(const b3Quaternion& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif
// template <typename b3Scalar>
// explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
/**@brief Constructor from scalars */
b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
: b3QuadWord(_x, _y, _z, _w)
{
//b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
* @param angle The magnitude of the rotation around the angle (Radians) */
b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
{
setRotation(_axis, _angle);
}
/**@brief Constructor from Euler angles
* @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
#ifndef B3_EULER_DEFAULT_ZYX
setEuler(yaw, pitch, roll);
#else
setEulerZYX(yaw, pitch, roll);
#endif
}
/**@brief Set the rotation using axis angle notation
* @param axis The axis around which to rotate
* @param angle The magnitude of the rotation in Radians */
void setRotation(const b3Vector3& axis1, const b3Scalar& _angle)
{
b3Vector3 axis = axis1;
axis.safeNormalize();
b3Scalar d = axis.length();
b3Assert(d != b3Scalar(0.0));
if (d < B3_EPSILON)
{
setValue(0, 0, 0, 1);
}
else
{
b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
b3Cos(_angle * b3Scalar(0.5)));
}
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
* @param pitch Angle around X
* @param roll Angle around Z */
void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch);
b3Scalar sinPitch = b3Sin(halfPitch);
b3Scalar cosRoll = b3Cos(halfRoll);
b3Scalar sinRoll = b3Sin(halfRoll);
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
}
/**@brief Set the quaternion using euler angles
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
{
b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch);
b3Scalar sinPitch = b3Sin(halfPitch);
b3Scalar cosRoll = b3Cos(halfRoll);
b3Scalar sinRoll = b3Sin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
normalize();
}
/**@brief Get the euler angles from this quaternion
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
{
b3Scalar squ;
b3Scalar sqx;
b3Scalar sqy;
b3Scalar sqz;
b3Scalar sarg;
sqx = m_floats[0] * m_floats[0];
sqy = m_floats[1] * m_floats[1];
sqz = m_floats[2] * m_floats[2];
squ = m_floats[3] * m_floats[3];
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI : (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
}
/**@brief Add two quaternions
* @param q The quaternion to add to this one */
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
mVec128 = _mm_add_ps(mVec128, q.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, q.mVec128);
#else
m_floats[0] += q.getX();
m_floats[1] += q.getY();
m_floats[2] += q.getZ();
m_floats[3] += q.m_floats[3];
#endif
return *this;
}
/**@brief Subtract out a quaternion
* @param q The quaternion to subtract from this one */
b3Quaternion& operator-=(const b3Quaternion& q)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(mVec128, q.mVec128);
#else
m_floats[0] -= q.getX();
m_floats[1] -= q.getY();
m_floats[2] -= q.getZ();
m_floats[3] -= q.m_floats[3];
#endif
return *this;
}
/**@brief Scale this quaternion
* @param s The scalar to scale by */
b3Quaternion& operator*=(const b3Scalar& s)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = b3_pshufd_ps(vs, 0); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs);
#elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
m_floats[1] *= s;
m_floats[2] *= s;
m_floats[3] *= s;
#endif
return *this;
}
/**@brief Multiply this quaternion by q on the right
* @param q The other quaternion
* Equivilant to this = this * q */
b3Quaternion& operator*=(const b3Quaternion& q)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vQ2 = q.get128();
__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0, 1, 2, 0));
__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));
A1 = A1 * B1;
__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 1));
__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
A2 = A2 * B2;
B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2, 0, 1, 2));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
B1 = B1 * B2; // A3 *= B3
mVec128 = b3_splat_ps(mVec128, 3); // A0
mVec128 = mVec128 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
mVec128 = mVec128 + A1; // AB03 + AB12
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = mVec128;
float32x4_t vQ2 = q.get128();
float32x4_t A0, A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
mVec128 = A0;
#else
setValue(
m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
#endif
return *this;
}
/**@brief Return the dot product between this quaternion and another
* @param q The other quaternion */
b3Scalar dot(const b3Quaternion& q) const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, q.mVec128);
__m128 t = _mm_movehl_ps(vd, vd);
vd = _mm_add_ps(vd, t);
t = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, t);
return _mm_cvtss_f32(vd);
#elif defined(B3_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
x = vpadd_f32(x, x);
return vget_lane_f32(x, 0);
#else
return m_floats[0] * q.getX() +
m_floats[1] * q.getY() +
m_floats[2] * q.getZ() +
m_floats[3] * q.m_floats[3];
#endif
}
/**@brief Return the length squared of the quaternion */
b3Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the quaternion */
b3Scalar length() const
{
return b3Sqrt(length2());
}
/**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
b3Quaternion& normalize()
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, mVec128);
__m128 t = _mm_movehl_ps(vd, vd);
vd = _mm_add_ps(vd, t);
t = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, t);
vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(b3vOnes, vd);
vd = b3_pshufd_ps(vd, 0); // splat
mVec128 = _mm_mul_ps(mVec128, vd);
return *this;
#else
return *this /= length();
#endif
}
/**@brief Return a scaled version of this quaternion
* @param s The scale factor */
B3_FORCE_INLINE b3Quaternion
operator*(const b3Scalar& s) const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
return b3Quaternion(_mm_mul_ps(mVec128, vs));
#elif defined(B3_USE_NEON)
return b3Quaternion(vmulq_n_f32(mVec128, s));
#else
return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
#endif
}
/**@brief Return an inversely scaled versionof this quaternion
* @param s The inverse scale factor */
b3Quaternion operator/(const b3Scalar& s) const
{
b3Assert(s != b3Scalar(0.0));
return *this * (b3Scalar(1.0) / s);
}
/**@brief Inversely scale this quaternion
* @param s The scale factor */
b3Quaternion& operator/=(const b3Scalar& s)
{
b3Assert(s != b3Scalar(0.0));
return *this *= b3Scalar(1.0) / s;
}
/**@brief Return a normalized version of this quaternion */
b3Quaternion normalized() const
{
return *this / length();
}
/**@brief Return the angle between this quaternion and the other
* @param q The other quaternion */
b3Scalar angle(const b3Quaternion& q) const
{
b3Scalar s = b3Sqrt(length2() * q.length2());
b3Assert(s != b3Scalar(0.0));
return b3Acos(dot(q) / s);
}
/**@brief Return the angle of rotation represented by this quaternion */
b3Scalar getAngle() const
{
b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
return s;
}
/**@brief Return the axis of the rotation represented by this quaternion */
b3Vector3 getAxis() const
{
b3Scalar s_squared = 1.f - m_floats[3] * m_floats[3];
if (s_squared < b3Scalar(10.) * B3_EPSILON) //Check for divide by zero
return b3MakeVector3(1.0, 0.0, 0.0); // Arbitrary
b3Scalar s = 1.f / b3Sqrt(s_squared);
return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
}
/**@brief Return the inverse of this quaternion */
b3Quaternion inverse() const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv));
#elif defined(B3_USE_NEON)
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv));
#else
return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
#endif
}
/**@brief Return the sum of this quaternion and the other
* @param q2 The other quaternion */
B3_FORCE_INLINE b3Quaternion
operator+(const b3Quaternion& q2) const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
#elif defined(B3_USE_NEON)
return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
#else
const b3Quaternion& q1 = *this;
return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
#endif
}
/**@brief Return the difference between this quaternion and the other
* @param q2 The other quaternion */
B3_FORCE_INLINE b3Quaternion
operator-(const b3Quaternion& q2) const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
#elif defined(B3_USE_NEON)
return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
#else
const b3Quaternion& q1 = *this;
return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
#endif
}
/**@brief Return the negative of this quaternion
* This simply negates each element */
B3_FORCE_INLINE b3Quaternion operator-() const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
#elif defined(B3_USE_NEON)
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask));
#else
const b3Quaternion& q2 = *this;
return b3Quaternion(-q2.getX(), -q2.getY(), -q2.getZ(), -q2.m_floats[3]);
#endif
}
/**@todo document this and it's use */
B3_FORCE_INLINE b3Quaternion farthest(const b3Quaternion& qd) const
{
b3Quaternion diff, sum;
diff = *this - qd;
sum = *this + qd;
if (diff.dot(diff) > sum.dot(sum))
return qd;
return (-qd);
}
/**@todo document this and it's use */
B3_FORCE_INLINE b3Quaternion nearest(const b3Quaternion& qd) const
{
b3Quaternion diff, sum;
diff = *this - qd;
sum = *this + qd;
if (diff.dot(diff) < sum.dot(sum))
return qd;
return (-qd);
}
/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
{
b3Scalar magnitude = b3Sqrt(length2() * q.length2());
b3Assert(magnitude > b3Scalar(0));
b3Scalar product = dot(q) / magnitude;
if (b3Fabs(product) < b3Scalar(1))
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
const b3Scalar theta = b3Acos(sign * product);
const b3Scalar s1 = b3Sin(sign * t * theta);
const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
return b3Quaternion(
(m_floats[0] * s0 + q.getX() * s1) * d,
(m_floats[1] * s0 + q.getY() * s1) * d,
(m_floats[2] * s0 + q.getZ() * s1) * d,
(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
}
else
{
return *this;
}
}
static const b3Quaternion& getIdentity()
{
static const b3Quaternion identityQuat(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.), b3Scalar(1.));
return identityQuat;
}
B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
};
/**@brief Return the product of two quaternions */
B3_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vQ1 = q1.get128();
__m128 vQ2 = q2.get128();
__m128 A0, A1, B1, A2, B2;
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0)); // X Y z x // vtrn
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0)); // W W W X // vdup vext
A1 = A1 * B1;
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1)); // Y Z X Y // vext
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1)); // z x Y Y // vtrn vdup
A2 = A2 * B2;
B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2)); // z x Y Z // vtrn vext
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2)); // Y Z x z // vext vtrn
B1 = B1 * B2; // A3 *= B3
A0 = b3_splat_ps(vQ1, 3); // A0
A0 = A0 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
A0 = A0 - B1; // AB03 = AB0 - AB3
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
A0 = A0 + A1; // AB03 + AB12
return b3Quaternion(A0);
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = q1.get128();
float32x4_t vQ2 = q2.get128();
float32x4_t A0, A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
return b3Quaternion(A0);
#else
return b3Quaternion(
q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
#endif
}
B3_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q, const b3Vector3& w)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vQ1 = q.get128();
__m128 vQ2 = w.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3, 3, 3, 0));
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0, 1, 2, 0));
A1 = A1 * B1;
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
A2 = A2 * B2;
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
A3 = A3 * B3; // A3 *= B3
A1 = A1 + A2; // AB12
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return b3Quaternion(A1);
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = q.get128();
float32x4_t vQ2 = w.get128();
float32x4_t A1, B1, A2, B2, A3, B3;
float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
{
float32x2x2_t tmp;
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
vQ2zx = tmp.val[0];
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
vQ1zx = tmp.val[0];
}
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return b3Quaternion(A1);
#else
return b3Quaternion(
q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
#endif
}
B3_FORCE_INLINE b3Quaternion
operator*(const b3Vector3& w, const b3Quaternion& q)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
__m128 vQ1 = w.get128();
__m128 vQ2 = q.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0)); // X Y z x
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0)); // W W W X
A1 = A1 * B1;
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
A2 = A2 * B2;
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
A3 = A3 * B3; // A3 *= B3
A1 = A1 + A2; // AB12
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return b3Quaternion(A1);
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = w.get128();
float32x4_t vQ2 = q.get128();
float32x4_t A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return b3Quaternion(A1);
#else
return b3Quaternion(
+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
#endif
}
/**@brief Calculate the dot product between two quaternions */
B3_FORCE_INLINE b3Scalar
b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
{
return q1.dot(q2);
}
/**@brief Return the length of a quaternion */
B3_FORCE_INLINE b3Scalar
b3Length(const b3Quaternion& q)
{
return q.length();
}
/**@brief Return the angle between two quaternions*/
B3_FORCE_INLINE b3Scalar
b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
{
return q1.angle(q2);
}
/**@brief Return the inverse of a quaternion*/
B3_FORCE_INLINE b3Quaternion
b3Inverse(const b3Quaternion& q)
{
return q.inverse();
}
/**@brief Return the result of spherical linear interpolation betwen two quaternions
* @param q1 The first quaternion
* @param q2 The second quaternion
* @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
* Slerp assumes constant velocity between positions. */
B3_FORCE_INLINE b3Quaternion
b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
{
return q1.slerp(q2, t);
}
B3_FORCE_INLINE b3Quaternion
b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
{
return rot0 * rot1;
}
B3_FORCE_INLINE b3Quaternion
b3QuatNormalized(const b3Quaternion& orn)
{
return orn.normalized();
}
B3_FORCE_INLINE b3Vector3
b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
{
b3Quaternion q = rotation * v;
q *= rotation.inverse();
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
#elif defined(B3_USE_NEON)
return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
#else
return b3MakeVector3(q.getX(), q.getY(), q.getZ());
#endif
}
B3_FORCE_INLINE b3Quaternion
b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
{
b3Vector3 c = v0.cross(v1);
b3Scalar d = v0.dot(v1);
if (d < -1.0 + B3_EPSILON)
{
b3Vector3 n, unused;
b3PlaneSpace1(v0, n, unused);
return b3Quaternion(n.getX(), n.getY(), n.getZ(), 0.0f); // just pick any vector that is orthogonal to v0
}
b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
b3Scalar rs = 1.0f / s;
return b3Quaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
}
B3_FORCE_INLINE b3Quaternion
b3ShortestArcQuatNormalize2(b3Vector3& v0, b3Vector3& v1)
{
v0.normalize();
v1.normalize();
return b3ShortestArcQuat(v0, v1);
}
#endif //B3_SIMD__QUATERNION_H_

48
thirdparty/bullet/Bullet3Common/b3Random.h vendored
View File

@ -1,48 +0,0 @@
#ifndef B3_GEN_RANDOM_H
#define B3_GEN_RANDOM_H
/*
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.
*/
#include "b3Scalar.h"
#ifdef MT19937
#include <limits.h>
#include <mt19937.h>
#define B3_RAND_MAX UINT_MAX
B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
#else
#include <stdlib.h>
#define B3_RAND_MAX RAND_MAX
B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
#endif
inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
{
return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
}
#endif //B3_GEN_RANDOM_H

171
thirdparty/bullet/Bullet3Common/b3ResizablePool.h vendored
View File

@ -1,171 +0,0 @@
#ifndef B3_RESIZABLE_POOL_H
#define B3_RESIZABLE_POOL_H
#include "Bullet3Common/b3AlignedObjectArray.h"
enum
{
B3_POOL_HANDLE_TERMINAL_FREE = -1,
B3_POOL_HANDLE_TERMINAL_USED = -2
};
template <typename U>
struct b3PoolBodyHandle : public U
{
B3_DECLARE_ALIGNED_ALLOCATOR();
int m_nextFreeHandle;
void setNextFree(int next)
{
m_nextFreeHandle = next;
}
int getNextFree() const
{
return m_nextFreeHandle;
}
};
template <typename T>
class b3ResizablePool
{
protected:
b3AlignedObjectArray<T> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
T* getHandleInternal(int handle)
{
return &m_bodyHandles[handle];
}
const T* getHandleInternal(int handle) const
{
return &m_bodyHandles[handle];
}
public:
b3ResizablePool()
{
initHandles();
}
virtual ~b3ResizablePool()
{
exitHandles();
}
///handle management
int getNumHandles() const
{
return m_bodyHandles.size();
}
void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
{
for (int i = 0; i < m_bodyHandles.size(); i++)
{
if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
usedHandles.push_back(i);
}
}
}
T* getHandle(int handle)
{
b3Assert(handle >= 0);
b3Assert(handle < m_bodyHandles.size());
if ((handle < 0) || (handle >= m_bodyHandles.size()))
{
return 0;
}
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
return &m_bodyHandles[handle];
}
return 0;
}
const T* getHandle(int handle) const
{
b3Assert(handle >= 0);
b3Assert(handle < m_bodyHandles.size());
if ((handle < 0) || (handle >= m_bodyHandles.size()))
{
return 0;
}
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
return &m_bodyHandles[handle];
}
return 0;
}
void increaseHandleCapacity(int extraCapacity)
{
int curCapacity = m_bodyHandles.size();
//b3Assert(curCapacity == m_numUsedHandles);
int newCapacity = curCapacity + extraCapacity;
m_bodyHandles.resize(newCapacity);
{
for (int i = curCapacity; i < newCapacity; i++)
m_bodyHandles[i].setNextFree(i + 1);
m_bodyHandles[newCapacity - 1].setNextFree(-1);
}
m_firstFreeHandle = curCapacity;
}
void initHandles()
{
m_numUsedHandles = 0;
m_firstFreeHandle = -1;
increaseHandleCapacity(1);
}
void exitHandles()
{
m_bodyHandles.resize(0);
m_firstFreeHandle = -1;
m_numUsedHandles = 0;
}
int allocHandle()
{
b3Assert(m_firstFreeHandle >= 0);
int handle = m_firstFreeHandle;
m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
m_numUsedHandles++;
if (m_firstFreeHandle < 0)
{
//int curCapacity = m_bodyHandles.size();
int additionalCapacity = m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
}
getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
getHandleInternal(handle)->clear();
return handle;
}
void freeHandle(int handle)
{
b3Assert(handle >= 0);
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
{
getHandleInternal(handle)->clear();
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
m_numUsedHandles--;
}
}
};
///end handle management
#endif //B3_RESIZABLE_POOL_H

Some files were not shown because too many files have changed in this diff Show More