godot/modules/bullet/rigid_body_bullet.cpp

1010 lines
32 KiB
C++
Raw Normal View History

/*************************************************************************/
/* body_bullet.cpp */
/* Author: AndreaCatania */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2017 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 "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btBulletCollisionCommon.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 <assert.h>
BulletPhysicsDirectBodyState *BulletPhysicsDirectBodyState::singleton = NULL;
Vector3 BulletPhysicsDirectBodyState::get_total_gravity() const {
Vector3 gVec;
B_TO_G(body->btBody->getGravity(), gVec);
return gVec;
}
float BulletPhysicsDirectBodyState::get_total_angular_damp() const {
return body->btBody->getAngularDamping();
}
float BulletPhysicsDirectBodyState::get_total_linear_damp() const {
return body->btBody->getLinearDamping();
}
Vector3 BulletPhysicsDirectBodyState::get_center_of_mass() const {
Vector3 gVec;
B_TO_G(body->btBody->getCenterOfMassPosition(), gVec);
return gVec;
}
Basis BulletPhysicsDirectBodyState::get_principal_inertia_axes() const {
return Basis();
}
float BulletPhysicsDirectBodyState::get_inverse_mass() const {
return body->btBody->getInvMass();
}
Vector3 BulletPhysicsDirectBodyState::get_inverse_inertia() const {
Vector3 gVec;
B_TO_G(body->btBody->getInvInertiaDiagLocal(), gVec);
return gVec;
}
Basis BulletPhysicsDirectBodyState::get_inverse_inertia_tensor() const {
Basis gInertia;
B_TO_G(body->btBody->getInvInertiaTensorWorld(), gInertia);
return gInertia;
}
void BulletPhysicsDirectBodyState::set_linear_velocity(const Vector3 &p_velocity) {
body->set_linear_velocity(p_velocity);
}
Vector3 BulletPhysicsDirectBodyState::get_linear_velocity() const {
return body->get_linear_velocity();
}
void BulletPhysicsDirectBodyState::set_angular_velocity(const Vector3 &p_velocity) {
body->set_angular_velocity(p_velocity);
}
Vector3 BulletPhysicsDirectBodyState::get_angular_velocity() const {
return body->get_angular_velocity();
}
void BulletPhysicsDirectBodyState::set_transform(const Transform &p_transform) {
body->set_transform(p_transform);
}
Transform BulletPhysicsDirectBodyState::get_transform() const {
return body->get_transform();
}
void BulletPhysicsDirectBodyState::add_force(const Vector3 &p_force, const Vector3 &p_pos) {
body->apply_force(p_force, p_pos);
}
void BulletPhysicsDirectBodyState::apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) {
body->apply_impulse(p_pos, p_j);
}
void BulletPhysicsDirectBodyState::apply_torque_impulse(const Vector3 &p_j) {
body->apply_torque_impulse(p_j);
}
void BulletPhysicsDirectBodyState::set_sleep_state(bool p_enable) {
body->set_activation_state(p_enable);
}
bool BulletPhysicsDirectBodyState::is_sleeping() const {
return !body->is_active();
}
int BulletPhysicsDirectBodyState::get_contact_count() const {
return body->collisionsCount;
}
Vector3 BulletPhysicsDirectBodyState::get_contact_local_position(int p_contact_idx) const {
return body->collisions[p_contact_idx].hitLocalLocation;
}
Vector3 BulletPhysicsDirectBodyState::get_contact_local_normal(int p_contact_idx) const {
return body->collisions[p_contact_idx].hitNormal;
}
int BulletPhysicsDirectBodyState::get_contact_local_shape(int p_contact_idx) const {
return body->collisions[p_contact_idx].local_shape;
}
RID BulletPhysicsDirectBodyState::get_contact_collider(int p_contact_idx) const {
return body->collisions[p_contact_idx].otherObject->get_self();
}
Vector3 BulletPhysicsDirectBodyState::get_contact_collider_position(int p_contact_idx) const {
return body->collisions[p_contact_idx].hitWorldLocation;
}
ObjectID BulletPhysicsDirectBodyState::get_contact_collider_id(int p_contact_idx) const {
return body->collisions[p_contact_idx].otherObject->get_instance_id();
}
int BulletPhysicsDirectBodyState::get_contact_collider_shape(int p_contact_idx) const {
return body->collisions[p_contact_idx].other_object_shape;
}
Vector3 BulletPhysicsDirectBodyState::get_contact_collider_velocity_at_position(int p_contact_idx) const {
RigidBodyBullet::CollisionData &colDat = body->collisions[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;
}
PhysicsDirectSpaceState *BulletPhysicsDirectBodyState::get_space_state() {
return body->get_space()->get_direct_state();
}
RigidBodyBullet::KinematicUtilities::KinematicUtilities(RigidBodyBullet *p_owner)
: m_owner(p_owner), m_margin(0.01) // Godot default margin 0.001
{
m_ghostObject = bulletnew(btPairCachingGhostObject);
int clearedCurrentFlags = m_ghostObject->getCollisionFlags();
clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
m_ghostObject->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT);
m_ghostObject->setUserPointer(p_owner);
m_ghostObject->setUserIndex(TYPE_KINEMATIC_GHOST_BODY);
resetDefShape();
}
RigidBodyBullet::KinematicUtilities::~KinematicUtilities() {
just_delete_shapes(m_shapes.size()); // don't need to resize
bulletdelete(m_ghostObject);
}
void RigidBodyBullet::KinematicUtilities::resetDefShape() {
m_ghostObject->setCollisionShape(BulletPhysicsServer::get_empty_shape());
}
void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(m_owner->get_shapes_wrappers());
const int shapes_count = shapes_wrappers.size();
just_delete_shapes(shapes_count);
const CollisionObjectBullet::ShapeWrapper *shape_wrapper;
for (int i = shapes_count - 1; 0 <= i; --i) {
shape_wrapper = &shapes_wrappers[i];
if (!shape_wrapper->active) {
continue;
}
m_shapes[i].transform = shape_wrapper->transform;
btConvexShape *&kin_shape_ref = m_shapes[i].shape;
switch (shape_wrapper->shape->get_type()) {
case PhysicsServer::SHAPE_SPHERE: {
SphereShapeBullet *sphere = static_cast<SphereShapeBullet *>(shape_wrapper->shape);
kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * m_owner->body_scale[0] + m_margin);
break;
}
case PhysicsServer::SHAPE_BOX: {
BoxShapeBullet *box = static_cast<BoxShapeBullet *>(shape_wrapper->shape);
kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * m_owner->body_scale) + btVector3(m_margin, m_margin, m_margin));
break;
}
case PhysicsServer::SHAPE_CAPSULE: {
CapsuleShapeBullet *capsule = static_cast<CapsuleShapeBullet *>(shape_wrapper->shape);
kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * m_owner->body_scale[0] + m_margin, capsule->get_height() * m_owner->body_scale[1] + m_margin);
break;
}
case PhysicsServer::SHAPE_CONVEX_POLYGON: {
ConvexPolygonShapeBullet *godot_convex = static_cast<ConvexPolygonShapeBullet *>(shape_wrapper->shape);
kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices);
kin_shape_ref->setLocalScaling(m_owner->body_scale + btVector3(m_margin, m_margin, m_margin));
break;
}
case PhysicsServer::SHAPE_RAY: {
RayShapeBullet *godot_ray = static_cast<RayShapeBullet *>(shape_wrapper->shape);
kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * m_owner->body_scale[1] + m_margin);
break;
}
default:
WARN_PRINT("This shape is not supported to be kinematic!");
kin_shape_ref = NULL;
}
}
}
void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) {
for (int i = m_shapes.size() - 1; 0 <= i; --i) {
if (m_shapes[i].shape) {
bulletdelete(m_shapes[i].shape);
}
}
m_shapes.resize(new_size);
}
RigidBodyBullet::RigidBodyBullet()
: RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_RIGID_BODY),
kinematic_utilities(NULL),
gravity_scale(1),
mass(1),
linearDamp(0),
angularDamp(0),
can_sleep(true),
force_integration_callback(NULL),
isTransformChanged(false),
maxCollisionsDetection(0),
collisionsCount(0),
maxAreasWhereIam(10),
areaWhereIamCount(0),
countGravityPointSpaces(0),
isScratchedSpaceOverrideModificator(false) {
godotMotionState = bulletnew(GodotMotionState(this));
// Initial properties
const btVector3 localInertia(0, 0, 0);
btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, compoundShape, localInertia);
btBody = bulletnew(btRigidBody(cInfo));
setupBulletCollisionObject(btBody);
set_mode(PhysicsServer::BODY_MODE_RIGID);
set_axis_lock(PhysicsServer::BODY_AXIS_LOCK_DISABLED);
areasWhereIam.resize(maxAreasWhereIam);
for (int i = areasWhereIam.size() - 1; 0 <= i; --i) {
areasWhereIam[i] = NULL;
}
}
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 = NULL;
}
}
void RigidBodyBullet::reload_body() {
if (space) {
space->remove_rigid_body(this);
space->add_rigid_body(this);
}
}
void RigidBodyBullet::set_space(SpaceBullet *p_space) {
// Clear the old space if there is one
if (space) {
isTransformChanged = 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 isTransformChanged is necessary in order to call integrated forces only when the first transform is sent
if (btBody->isActive() && force_integration_callback && isTransformChanged) {
BulletPhysicsDirectBodyState *bodyDirect = BulletPhysicsDirectBodyState::get_singleton(this);
Variant variantBodyDirect = bodyDirect;
Object *obj = ObjectDB::get_instance(force_integration_callback->id);
if (!obj) {
// Remove integration callback
set_force_integration_callback(0, StringName());
} else {
const Variant *vp[2] = { &variantBodyDirect, &force_integration_callback->udata };
Variant::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());
}
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 = NULL;
}
if (p_id != 0) {
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() {
isTransformChanged = true;
}
void RigidBodyBullet::scratch_space_override_modificator() {
isScratchedSpaceOverrideModificator = true;
}
void RigidBodyBullet::on_collision_filters_change() {
if (space) {
space->reload_collision_filters(this);
}
}
void RigidBodyBullet::on_collision_checker_start() {
collisionsCount = 0;
}
bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, int p_other_shape_index, int p_local_shape_index) {
if (collisionsCount >= maxCollisionsDetection) {
return false;
}
CollisionData &cd = collisions[collisionsCount];
cd.hitLocalLocation = p_hitLocalLocation;
cd.otherObject = p_otherObject;
cd.hitWorldLocation = p_hitWorldLocation;
cd.hitNormal = p_hitNormal;
cd.other_object_shape = p_other_shape_index;
cd.local_shape = p_local_shape_index;
++collisionsCount;
return true;
}
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->setActivationState(ACTIVE_TAG);
} else {
btBody->setActivationState(WANTS_DEACTIVATION);
}
}
bool RigidBodyBullet::is_active() const {
return btBody->isActive();
}
void RigidBodyBullet::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer::BODY_PARAM_BOUNCE:
btBody->setRestitution(p_value);
break;
case PhysicsServer::BODY_PARAM_FRICTION:
btBody->setFriction(p_value);
break;
case PhysicsServer::BODY_PARAM_MASS: {
ERR_FAIL_COND(p_value < 0);
mass = p_value;
_internal_set_mass(p_value);
break;
}
case PhysicsServer::BODY_PARAM_LINEAR_DAMP:
linearDamp = p_value;
btBody->setDamping(linearDamp, angularDamp);
break;
case PhysicsServer::BODY_PARAM_ANGULAR_DAMP:
angularDamp = p_value;
btBody->setDamping(linearDamp, angularDamp);
break;
case PhysicsServer::BODY_PARAM_GRAVITY_SCALE:
gravity_scale = p_value;
/// The Bullet gravity will be is set by reload_space_override_modificator
scratch_space_override_modificator();
break;
default:
WARN_PRINTS("Parameter " + itos(p_param) + " not supported by bullet. Value: " + itos(p_value));
}
}
real_t RigidBodyBullet::get_param(PhysicsServer::BodyParameter p_param) const {
switch (p_param) {
case PhysicsServer::BODY_PARAM_BOUNCE:
return btBody->getRestitution();
case PhysicsServer::BODY_PARAM_FRICTION:
return btBody->getFriction();
case PhysicsServer::BODY_PARAM_MASS: {
const btScalar invMass = btBody->getInvMass();
return 0 == invMass ? 0 : 1 / invMass;
}
case PhysicsServer::BODY_PARAM_LINEAR_DAMP:
return linearDamp;
case PhysicsServer::BODY_PARAM_ANGULAR_DAMP:
return angularDamp;
case PhysicsServer::BODY_PARAM_GRAVITY_SCALE:
return gravity_scale;
default:
WARN_PRINTS("Parameter " + itos(p_param) + " not supported by bullet");
return 0;
}
}
void RigidBodyBullet::set_mode(PhysicsServer::BodyMode p_mode) {
// This is necessary to block force_integration untile next move
isTransformChanged = false;
destroy_kinematic_utilities();
// The mode change is relevant to its mass
switch (p_mode) {
case PhysicsServer::BODY_MODE_KINEMATIC:
mode = PhysicsServer::BODY_MODE_KINEMATIC;
set_axis_lock(axis_lock); // Reload axis lock
_internal_set_mass(0);
init_kinematic_utilities();
break;
case PhysicsServer::BODY_MODE_STATIC:
mode = PhysicsServer::BODY_MODE_STATIC;
set_axis_lock(axis_lock); // Reload axis lock
_internal_set_mass(0);
break;
case PhysicsServer::BODY_MODE_RIGID: {
mode = PhysicsServer::BODY_MODE_RIGID;
set_axis_lock(axis_lock); // Reload axis lock
_internal_set_mass(0 == mass ? 1 : mass);
break;
}
case PhysicsServer::BODY_MODE_CHARACTER: {
mode = PhysicsServer::BODY_MODE_CHARACTER;
set_axis_lock(axis_lock); // Reload axis lock
_internal_set_mass(0 == mass ? 1 : mass);
break;
}
}
btBody->setAngularVelocity(btVector3(0, 0, 0));
btBody->setLinearVelocity(btVector3(0, 0, 0));
}
PhysicsServer::BodyMode RigidBodyBullet::get_mode() const {
return mode;
}
void RigidBodyBullet::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) {
switch (p_state) {
case PhysicsServer::BODY_STATE_TRANSFORM:
set_transform(p_variant);
break;
case PhysicsServer::BODY_STATE_LINEAR_VELOCITY:
set_linear_velocity(p_variant);
break;
case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY:
set_angular_velocity(p_variant);
break;
case PhysicsServer::BODY_STATE_SLEEPING:
set_activation_state(!bool(p_variant));
break;
case PhysicsServer::BODY_STATE_CAN_SLEEP:
can_sleep = bool(p_variant);
if (!can_sleep) {
// Can't sleep
btBody->forceActivationState(DISABLE_DEACTIVATION);
}
break;
}
}
Variant RigidBodyBullet::get_state(PhysicsServer::BodyState p_state) const {
switch (p_state) {
case PhysicsServer::BODY_STATE_TRANSFORM:
return get_transform();
case PhysicsServer::BODY_STATE_LINEAR_VELOCITY:
return get_linear_velocity();
case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY:
return get_angular_velocity();
case PhysicsServer::BODY_STATE_SLEEPING:
return !is_active();
case PhysicsServer::BODY_STATE_CAN_SLEEP:
return can_sleep;
default:
WARN_PRINTS("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);
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);
btBody->activate();
btBody->applyImpulse(btImpu, btPos);
}
void RigidBodyBullet::apply_torque_impulse(const Vector3 &p_impulse) {
btVector3 btImp;
G_TO_B(p_impulse, btImp);
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);
btBody->activate();
btBody->applyForce(btForce, btPos);
}
void RigidBodyBullet::apply_central_force(const Vector3 &p_force) {
btVector3 btForce;
G_TO_B(p_force, btForce);
btBody->activate();
btBody->applyCentralForce(btForce);
}
void RigidBodyBullet::apply_torque(const Vector3 &p_torque) {
btVector3 btTorq;
G_TO_B(p_torque, btTorq);
btBody->activate();
btBody->applyTorque(btTorq);
}
void RigidBodyBullet::set_applied_force(const Vector3 &p_force) {
btVector3 btVec = btBody->getTotalTorque();
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();
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(PhysicsServer::BodyAxisLock p_lock) {
axis_lock = p_lock;
if (PhysicsServer::BODY_AXIS_LOCK_DISABLED == axis_lock) {
btBody->setLinearFactor(btVector3(1., 1., 1.));
btBody->setAngularFactor(btVector3(1., 1., 1.));
} else if (PhysicsServer::BODY_AXIS_LOCK_X == axis_lock) {
btBody->setLinearFactor(btVector3(0., 1., 1.));
btBody->setAngularFactor(btVector3(1., 0., 0.));
} else if (PhysicsServer::BODY_AXIS_LOCK_Y == axis_lock) {
btBody->setLinearFactor(btVector3(1., 0., 1.));
btBody->setAngularFactor(btVector3(0., 1., 0.));
} else if (PhysicsServer::BODY_AXIS_LOCK_Z == axis_lock) {
btBody->setLinearFactor(btVector3(1., 1., 0.));
btBody->setAngularFactor(btVector3(0., 0., 1.));
}
if (PhysicsServer::BODY_MODE_CHARACTER == mode) {
/// When character lock angular
btBody->setAngularFactor(btVector3(0., 0., 0.));
}
}
PhysicsServer::BodyAxisLock RigidBodyBullet::get_axis_lock() const {
btVector3 vec = btBody->getLinearFactor();
if (0. == vec.x()) {
return PhysicsServer::BODY_AXIS_LOCK_X;
} else if (0. == vec.y()) {
return PhysicsServer::BODY_AXIS_LOCK_Y;
} else if (0. == vec.z()) {
return PhysicsServer::BODY_AXIS_LOCK_Z;
} else {
return PhysicsServer::BODY_AXIS_LOCK_DISABLED;
}
}
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(1);
/// 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 dimentions 1 meter, try 0.2
btVector3 center;
btScalar radius;
btBody->getCollisionShape()->getBoundingSphere(center, radius);
btBody->setCcdSweptSphereRadius(radius * 0.2);
} else {
btBody->setCcdMotionThreshold(0.);
btBody->setCcdSweptSphereRadius(0.);
}
}
bool RigidBodyBullet::is_continuous_collision_detection_enabled() const {
return 0. != btBody->getCcdMotionThreshold();
}
void RigidBodyBullet::set_linear_velocity(const Vector3 &p_velocity) {
btVector3 btVec;
G_TO_B(p_velocity, btVec);
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);
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 == PhysicsServer::BODY_MODE_KINEMATIC) {
// The kinematic use MotionState class
godotMotionState->moveBody(p_global_transform);
}
btBody->setWorldTransform(p_global_transform);
}
const btTransform &RigidBodyBullet::get_transform__bullet() const {
if (is_static()) {
return RigidCollisionObjectBullet::get_transform__bullet();
} else {
return godotMotionState->getCurrentWorldTransform();
}
}
void RigidBodyBullet::on_shapes_changed() {
RigidCollisionObjectBullet::on_shapes_changed();
const btScalar invMass = btBody->getInvMass();
const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
btVector3 inertia;
btBody->getCollisionShape()->calculateLocalInertia(mass, inertia);
btBody->setMassProps(mass, inertia);
btBody->updateInertiaTensor();
reload_kinematic_shapes();
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 (NULL == areasWhereIam[i]) {
// This area has the highest priority
areasWhereIam[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[j + 1] = areasWhereIam[j];
}
areasWhereIam[i] = p_area;
break;
}
}
}
if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
scratch_space_override_modificator();
}
if (p_area->is_spOv_gravityPoint()) {
++countGravityPointSpaces;
assert(0 < countGravityPointSpaces);
}
}
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 fount, just shift down all elements
for (int j = i; j < areaWhereIamCount; ++j) {
areasWhereIam[j] = areasWhereIam[j + 1];
}
wasTheAreaFound = true;
break;
}
}
if (wasTheAreaFound) {
if (p_area->is_spOv_gravityPoint()) {
--countGravityPointSpaces;
assert(0 <= countGravityPointSpaces);
}
--areaWhereIamCount;
areasWhereIam[areaWhereIamCount] = NULL; // Even if this is not required, I clear the last element to be safe
if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
scratch_space_override_modificator();
}
}
}
void RigidBodyBullet::reload_space_override_modificator() {
Vector3 newGravity(space->get_gravity_direction() * space->get_gravity_magnitude());
real_t newLinearDamp(linearDamp);
real_t newAngularDamp(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);
int countCombined(0);
for (int i = areaWhereIamCount - 1; 0 <= i; --i) {
currentArea = areasWhereIam[i];
if (PhysicsServer::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 PhysicsServer::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 PhysicsServer::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();
++countCombined;
break;
case PhysicsServer::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();
++countCombined;
goto endAreasCycle;
case PhysicsServer::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();
countCombined = 1;
goto endAreasCycle;
case PhysicsServer::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();
countCombined = 1;
break;
}
}
endAreasCycle:
if (1 < countCombined) {
newGravity /= countCombined;
newLinearDamp /= countCombined;
newAngularDamp /= countCombined;
}
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::_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) {
ERR_FAIL_COND(PhysicsServer::BODY_MODE_RIGID != mode && PhysicsServer::BODY_MODE_CHARACTER != mode);
m_isStatic = false;
compoundShape->calculateLocalInertia(p_mass, localInertia);
if (PhysicsServer::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 {
ERR_FAIL_COND(PhysicsServer::BODY_MODE_STATIC != mode && PhysicsServer::BODY_MODE_KINEMATIC != mode);
m_isStatic = true;
if (PhysicsServer::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();
}