maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
godot/modules/bullet/rigid_body_bullet.cpp
AndreaCatania fb4871c919 Bullet physics engine implementation 
This is a bullet wrapper that allows Godot to use Bullet physics and benefit about all features.
Also it support all specific Godot physics functionality like multi shape body, areas, RayShape, etc..
It improve the Joints, Trimesh shape, and add support to soft body even if Godot is not yet ready to it.
2017-11-04 20:52:59 +01:00

1010 lines
32 KiB
C++
Raw Blame 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();
}
Reference in a new issue View git blame Copy permalink