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