93ab45b6b5
They do not play well with clang-format which aligns the `//` part with the rest of the code block, thus producing badly indented commented code.
826 lines
20 KiB
C++
826 lines
20 KiB
C++
/*************************************************************************/
|
|
/* body_sw.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* http://www.godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* 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 "body_sw.h"
|
|
#include "space_sw.h"
|
|
#include "area_sw.h"
|
|
|
|
void BodySW::_update_inertia() {
|
|
|
|
if (get_space() && !inertia_update_list.in_list())
|
|
get_space()->body_add_to_inertia_update_list(&inertia_update_list);
|
|
|
|
}
|
|
|
|
void BodySW::_update_transform_dependant() {
|
|
|
|
center_of_mass = get_transform().basis.xform(center_of_mass_local);
|
|
principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
|
|
|
|
// update inertia tensor
|
|
Basis tb = principal_inertia_axes;
|
|
Basis tbt = tb.transposed();
|
|
tb.scale(_inv_inertia);
|
|
_inv_inertia_tensor = tb * tbt;
|
|
|
|
}
|
|
|
|
void BodySW::update_inertias() {
|
|
|
|
//update shapes and motions
|
|
|
|
switch(mode) {
|
|
|
|
case PhysicsServer::BODY_MODE_RIGID: {
|
|
|
|
//update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
|
|
float total_area=0;
|
|
|
|
for (int i=0;i<get_shape_count();i++) {
|
|
|
|
total_area+=get_shape_area(i);
|
|
}
|
|
|
|
// We have to recompute the center of mass
|
|
center_of_mass_local.zero();
|
|
|
|
for (int i=0; i<get_shape_count(); i++) {
|
|
float area=get_shape_area(i);
|
|
|
|
float mass = area * this->mass / total_area;
|
|
|
|
// NOTE: we assume that the shape origin is also its center of mass
|
|
center_of_mass_local += mass * get_shape_transform(i).origin;
|
|
}
|
|
|
|
center_of_mass_local /= mass;
|
|
|
|
// Recompute the inertia tensor
|
|
Basis inertia_tensor;
|
|
inertia_tensor.set_zero();
|
|
|
|
for (int i=0;i<get_shape_count();i++) {
|
|
|
|
const ShapeSW* shape=get_shape(i);
|
|
|
|
float area=get_shape_area(i);
|
|
|
|
float mass = area * this->mass / total_area;
|
|
|
|
Basis shape_inertia_tensor=shape->get_moment_of_inertia(mass).to_diagonal_matrix();
|
|
Transform shape_transform=get_shape_transform(i);
|
|
Basis shape_basis = shape_transform.basis.orthonormalized();
|
|
|
|
// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
|
|
shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
|
|
|
|
Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
|
|
inertia_tensor += shape_inertia_tensor + (Basis()*shape_origin.dot(shape_origin)-shape_origin.outer(shape_origin))*mass;
|
|
|
|
|
|
}
|
|
|
|
// Compute the principal axes of inertia
|
|
principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
|
|
_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
|
|
|
|
if (mass)
|
|
_inv_mass=1.0/mass;
|
|
else
|
|
_inv_mass=0;
|
|
|
|
} break;
|
|
|
|
case PhysicsServer::BODY_MODE_KINEMATIC:
|
|
case PhysicsServer::BODY_MODE_STATIC: {
|
|
|
|
_inv_inertia_tensor.set_zero();
|
|
_inv_mass=0;
|
|
} break;
|
|
case PhysicsServer::BODY_MODE_CHARACTER: {
|
|
|
|
_inv_inertia_tensor.set_zero();
|
|
_inv_mass=1.0/mass;
|
|
|
|
} break;
|
|
}
|
|
|
|
|
|
//_update_shapes();
|
|
|
|
_update_transform_dependant();
|
|
}
|
|
|
|
|
|
|
|
void BodySW::set_active(bool p_active) {
|
|
|
|
if (active==p_active)
|
|
return;
|
|
|
|
active=p_active;
|
|
if (!p_active) {
|
|
if (get_space())
|
|
get_space()->body_remove_from_active_list(&active_list);
|
|
} else {
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC)
|
|
return; //static bodies can't become active
|
|
if (get_space())
|
|
get_space()->body_add_to_active_list(&active_list);
|
|
|
|
//still_time=0;
|
|
}
|
|
/*
|
|
if (!space)
|
|
return;
|
|
|
|
for(int i=0;i<get_shape_count();i++) {
|
|
Shape &s=shapes[i];
|
|
if (s.bpid>0) {
|
|
get_space()->get_broadphase()->set_active(s.bpid,active);
|
|
}
|
|
}
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {
|
|
|
|
switch(p_param) {
|
|
case PhysicsServer::BODY_PARAM_BOUNCE: {
|
|
|
|
bounce=p_value;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_FRICTION: {
|
|
|
|
friction=p_value;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_MASS: {
|
|
ERR_FAIL_COND(p_value<=0);
|
|
mass=p_value;
|
|
_update_inertia();
|
|
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: {
|
|
gravity_scale=p_value;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_LINEAR_DAMP: {
|
|
|
|
linear_damp=p_value;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: {
|
|
|
|
angular_damp=p_value;
|
|
} break;
|
|
default:{}
|
|
}
|
|
}
|
|
|
|
float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
|
|
|
|
switch(p_param) {
|
|
case PhysicsServer::BODY_PARAM_BOUNCE: {
|
|
|
|
return bounce;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_FRICTION: {
|
|
|
|
return friction;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_MASS: {
|
|
return mass;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: {
|
|
return gravity_scale;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_LINEAR_DAMP: {
|
|
|
|
return linear_damp;
|
|
} break;
|
|
case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: {
|
|
|
|
return angular_damp;
|
|
} break;
|
|
|
|
default:{}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
|
|
|
|
PhysicsServer::BodyMode prev=mode;
|
|
mode=p_mode;
|
|
|
|
switch(p_mode) {
|
|
//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
|
|
case PhysicsServer::BODY_MODE_STATIC:
|
|
case PhysicsServer::BODY_MODE_KINEMATIC: {
|
|
|
|
_set_inv_transform(get_transform().affine_inverse());
|
|
_inv_mass=0;
|
|
_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
|
|
//set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
|
|
set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC && contacts.size());
|
|
linear_velocity=Vector3();
|
|
angular_velocity=Vector3();
|
|
if (mode==PhysicsServer::BODY_MODE_KINEMATIC && prev!=mode) {
|
|
first_time_kinematic=true;
|
|
}
|
|
|
|
} break;
|
|
case PhysicsServer::BODY_MODE_RIGID: {
|
|
|
|
_inv_mass=mass>0?(1.0/mass):0;
|
|
_set_static(false);
|
|
|
|
} break;
|
|
case PhysicsServer::BODY_MODE_CHARACTER: {
|
|
|
|
_inv_mass=mass>0?(1.0/mass):0;
|
|
_set_static(false);
|
|
} break;
|
|
}
|
|
|
|
_update_inertia();
|
|
/*
|
|
if (get_space())
|
|
_update_queries();
|
|
*/
|
|
|
|
}
|
|
PhysicsServer::BodyMode BodySW::get_mode() const {
|
|
|
|
return mode;
|
|
}
|
|
|
|
void BodySW::_shapes_changed() {
|
|
|
|
_update_inertia();
|
|
}
|
|
|
|
void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {
|
|
|
|
switch(p_state) {
|
|
case PhysicsServer::BODY_STATE_TRANSFORM: {
|
|
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
|
|
new_transform=p_variant;
|
|
//wakeup_neighbours();
|
|
set_active(true);
|
|
if (first_time_kinematic) {
|
|
_set_transform(p_variant);
|
|
_set_inv_transform(get_transform().affine_inverse());
|
|
first_time_kinematic=false;
|
|
}
|
|
|
|
} else if (mode==PhysicsServer::BODY_MODE_STATIC) {
|
|
_set_transform(p_variant);
|
|
_set_inv_transform(get_transform().affine_inverse());
|
|
wakeup_neighbours();
|
|
} else {
|
|
Transform t = p_variant;
|
|
t.orthonormalize();
|
|
new_transform=get_transform(); //used as old to compute motion
|
|
if (new_transform==t)
|
|
break;
|
|
_set_transform(t);
|
|
_set_inv_transform(get_transform().inverse());
|
|
|
|
}
|
|
wakeup();
|
|
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
|
|
|
|
/*
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC)
|
|
break;
|
|
*/
|
|
linear_velocity=p_variant;
|
|
wakeup();
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
|
|
/*
|
|
if (mode!=PhysicsServer::BODY_MODE_RIGID)
|
|
break;
|
|
*/
|
|
angular_velocity=p_variant;
|
|
wakeup();
|
|
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_SLEEPING: {
|
|
//?
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
|
|
break;
|
|
bool do_sleep=p_variant;
|
|
if (do_sleep) {
|
|
linear_velocity=Vector3();
|
|
//biased_linear_velocity=Vector3();
|
|
angular_velocity=Vector3();
|
|
//biased_angular_velocity=Vector3();
|
|
set_active(false);
|
|
} else {
|
|
if (mode!=PhysicsServer::BODY_MODE_STATIC)
|
|
set_active(true);
|
|
}
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_CAN_SLEEP: {
|
|
can_sleep=p_variant;
|
|
if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
|
|
set_active(true);
|
|
|
|
} break;
|
|
}
|
|
|
|
}
|
|
Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {
|
|
|
|
switch(p_state) {
|
|
case PhysicsServer::BODY_STATE_TRANSFORM: {
|
|
return get_transform();
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
|
|
return linear_velocity;
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
|
|
return angular_velocity;
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_SLEEPING: {
|
|
return !is_active();
|
|
} break;
|
|
case PhysicsServer::BODY_STATE_CAN_SLEEP: {
|
|
return can_sleep;
|
|
} break;
|
|
}
|
|
|
|
return Variant();
|
|
}
|
|
|
|
|
|
void BodySW::set_space(SpaceSW *p_space){
|
|
|
|
if (get_space()) {
|
|
|
|
if (inertia_update_list.in_list())
|
|
get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
|
|
if (active_list.in_list())
|
|
get_space()->body_remove_from_active_list(&active_list);
|
|
if (direct_state_query_list.in_list())
|
|
get_space()->body_remove_from_state_query_list(&direct_state_query_list);
|
|
|
|
}
|
|
|
|
_set_space(p_space);
|
|
|
|
if (get_space()) {
|
|
|
|
_update_inertia();
|
|
if (active)
|
|
get_space()->body_add_to_active_list(&active_list);
|
|
/*
|
|
_update_queries();
|
|
if (is_active()) {
|
|
active=false;
|
|
set_active(true);
|
|
}
|
|
*/
|
|
|
|
}
|
|
|
|
first_integration=true;
|
|
|
|
}
|
|
|
|
void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) {
|
|
|
|
if (p_area->is_gravity_point()) {
|
|
if(p_area->get_gravity_distance_scale() > 0) {
|
|
Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
|
|
gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) );
|
|
} else {
|
|
gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
|
|
}
|
|
} else {
|
|
gravity += p_area->get_gravity_vector() * p_area->get_gravity();
|
|
}
|
|
|
|
area_linear_damp += p_area->get_linear_damp();
|
|
area_angular_damp += p_area->get_angular_damp();
|
|
}
|
|
|
|
void BodySW::integrate_forces(real_t p_step) {
|
|
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC)
|
|
return;
|
|
|
|
AreaSW *def_area = get_space()->get_default_area();
|
|
// AreaSW *damp_area = def_area;
|
|
|
|
ERR_FAIL_COND(!def_area);
|
|
|
|
int ac = areas.size();
|
|
bool stopped = false;
|
|
gravity = Vector3(0,0,0);
|
|
area_linear_damp = 0;
|
|
area_angular_damp = 0;
|
|
if (ac) {
|
|
areas.sort();
|
|
const AreaCMP *aa = &areas[0];
|
|
// damp_area = aa[ac-1].area;
|
|
for(int i=ac-1;i>=0 && !stopped;i--) {
|
|
PhysicsServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode();
|
|
switch (mode) {
|
|
case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE:
|
|
case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
|
|
_compute_area_gravity_and_dampenings(aa[i].area);
|
|
stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
|
|
} break;
|
|
case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE:
|
|
case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
|
|
gravity = Vector3(0,0,0);
|
|
area_angular_damp = 0;
|
|
area_linear_damp = 0;
|
|
_compute_area_gravity_and_dampenings(aa[i].area);
|
|
stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE;
|
|
} break;
|
|
default: {}
|
|
}
|
|
}
|
|
}
|
|
|
|
if( !stopped ) {
|
|
_compute_area_gravity_and_dampenings(def_area);
|
|
}
|
|
|
|
gravity*=gravity_scale;
|
|
|
|
// If less than 0, override dampenings with that of the Body
|
|
if (angular_damp>=0)
|
|
area_angular_damp=angular_damp;
|
|
/*
|
|
else
|
|
area_angular_damp=damp_area->get_angular_damp();
|
|
*/
|
|
|
|
if (linear_damp>=0)
|
|
area_linear_damp=linear_damp;
|
|
/*
|
|
else
|
|
area_linear_damp=damp_area->get_linear_damp();
|
|
*/
|
|
|
|
|
|
Vector3 motion;
|
|
bool do_motion=false;
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
|
|
|
|
//compute motion, angular and etc. velocities from prev transform
|
|
linear_velocity = (new_transform.origin - get_transform().origin)/p_step;
|
|
|
|
//compute a FAKE angular velocity, not so easy
|
|
Basis rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
|
|
Vector3 axis;
|
|
float angle;
|
|
|
|
rot.get_axis_and_angle(axis,angle);
|
|
axis.normalize();
|
|
angular_velocity=axis.normalized() * (angle/p_step);
|
|
|
|
motion = new_transform.origin - get_transform().origin;
|
|
do_motion=true;
|
|
|
|
} else {
|
|
if (!omit_force_integration && !first_integration) {
|
|
//overriden by direct state query
|
|
|
|
Vector3 force=gravity*mass;
|
|
force+=applied_force;
|
|
Vector3 torque=applied_torque;
|
|
|
|
real_t damp = 1.0 - p_step * area_linear_damp;
|
|
|
|
if (damp<0) // reached zero in the given time
|
|
damp=0;
|
|
|
|
real_t angular_damp = 1.0 - p_step * area_angular_damp;
|
|
|
|
if (angular_damp<0) // reached zero in the given time
|
|
angular_damp=0;
|
|
|
|
linear_velocity*=damp;
|
|
angular_velocity*=angular_damp;
|
|
|
|
linear_velocity+=_inv_mass * force * p_step;
|
|
angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
|
|
}
|
|
|
|
if (continuous_cd) {
|
|
motion=linear_velocity*p_step;
|
|
do_motion=true;
|
|
}
|
|
|
|
}
|
|
|
|
applied_force=Vector3();
|
|
applied_torque=Vector3();
|
|
first_integration=false;
|
|
|
|
//motion=linear_velocity*p_step;
|
|
|
|
biased_angular_velocity=Vector3();
|
|
biased_linear_velocity=Vector3();
|
|
|
|
|
|
if (do_motion) {//shapes temporarily extend for raycast
|
|
_update_shapes_with_motion(motion);
|
|
}
|
|
|
|
|
|
def_area=NULL; // clear the area, so it is set in the next frame
|
|
contact_count=0;
|
|
|
|
}
|
|
|
|
void BodySW::integrate_velocities(real_t p_step) {
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC)
|
|
return;
|
|
|
|
if (fi_callback)
|
|
get_space()->body_add_to_state_query_list(&direct_state_query_list);
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
|
|
|
|
_set_transform(new_transform,false);
|
|
_set_inv_transform(new_transform.affine_inverse());
|
|
if (contacts.size()==0 && linear_velocity==Vector3() && angular_velocity==Vector3())
|
|
set_active(false); //stopped moving, deactivate
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
|
|
//apply axis lock
|
|
if (axis_lock!=PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
|
|
|
|
|
|
int axis=axis_lock-1;
|
|
for(int i=0;i<3;i++) {
|
|
if (i==axis) {
|
|
linear_velocity[i]=0;
|
|
biased_linear_velocity[i]=0;
|
|
} else {
|
|
|
|
angular_velocity[i]=0;
|
|
biased_angular_velocity[i]=0;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;
|
|
|
|
|
|
|
|
float ang_vel = total_angular_velocity.length();
|
|
Transform transform = get_transform();
|
|
|
|
|
|
if (ang_vel!=0.0) {
|
|
Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
|
|
Basis rot( ang_vel_axis, -ang_vel*p_step );
|
|
Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
|
|
transform.basis = rot * transform.basis;
|
|
transform.orthonormalize();
|
|
}
|
|
|
|
Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;
|
|
/*for(int i=0;i<3;i++) {
|
|
if (axis_lock&(1<<i)) {
|
|
transform.origin[i]=0.0;
|
|
}
|
|
}*/
|
|
|
|
transform.origin+=total_linear_velocity * p_step;
|
|
|
|
_set_transform(transform);
|
|
_set_inv_transform(get_transform().inverse());
|
|
|
|
_update_transform_dependant();
|
|
|
|
/*
|
|
if (fi_callback) {
|
|
get_space()->body_add_to_state_query_list(&direct_state_query_list);
|
|
*/
|
|
}
|
|
|
|
/*
|
|
void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
|
|
|
|
Transform inv_xform = p_xform.affine_inverse();
|
|
if (!get_space()) {
|
|
_set_transform(p_xform);
|
|
_set_inv_transform(inv_xform);
|
|
|
|
return;
|
|
}
|
|
|
|
//compute a FAKE linear velocity - this is easy
|
|
|
|
linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
|
|
|
|
//compute a FAKE angular velocity, not so easy
|
|
Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
|
|
Vector3 axis;
|
|
float angle;
|
|
|
|
rot.get_axis_and_angle(axis,angle);
|
|
axis.normalize();
|
|
angular_velocity=axis.normalized() * (angle/p_step);
|
|
linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
|
|
|
|
if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
|
|
get_space()->body_add_to_state_query_list(&direct_state_query_list);
|
|
simulated_motion=true;
|
|
_set_transform(p_xform);
|
|
|
|
|
|
}
|
|
*/
|
|
|
|
void BodySW::wakeup_neighbours() {
|
|
|
|
for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
|
|
|
|
const ConstraintSW *c=E->key();
|
|
BodySW **n = c->get_body_ptr();
|
|
int bc=c->get_body_count();
|
|
|
|
for(int i=0;i<bc;i++) {
|
|
|
|
if (i==E->get())
|
|
continue;
|
|
BodySW *b = n[i];
|
|
if (b->mode!=PhysicsServer::BODY_MODE_RIGID)
|
|
continue;
|
|
|
|
if (!b->is_active())
|
|
b->set_active(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
void BodySW::call_queries() {
|
|
|
|
|
|
if (fi_callback) {
|
|
|
|
PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
|
|
dbs->body=this;
|
|
|
|
Variant v=dbs;
|
|
|
|
Object *obj = ObjectDB::get_instance(fi_callback->id);
|
|
if (!obj) {
|
|
|
|
set_force_integration_callback(0,StringName());
|
|
} else {
|
|
const Variant *vp[2]={&v,&fi_callback->udata};
|
|
|
|
Variant::CallError ce;
|
|
int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;
|
|
obj->call(fi_callback->method,vp,argc,ce);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
bool BodySW::sleep_test(real_t p_step) {
|
|
|
|
if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
|
|
return true; //
|
|
else if (mode==PhysicsServer::BODY_MODE_CHARACTER)
|
|
return !active; // characters don't sleep unless asked to sleep
|
|
else if (!can_sleep)
|
|
return false;
|
|
|
|
|
|
|
|
|
|
if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
|
|
|
|
still_time+=p_step;
|
|
|
|
return still_time > get_space()->get_body_time_to_sleep();
|
|
} else {
|
|
|
|
still_time=0; //maybe this should be set to 0 on set_active?
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
|
|
|
|
if (fi_callback) {
|
|
|
|
memdelete(fi_callback);
|
|
fi_callback=NULL;
|
|
}
|
|
|
|
|
|
if (p_id!=0) {
|
|
|
|
fi_callback=memnew(ForceIntegrationCallback);
|
|
fi_callback->id=p_id;
|
|
fi_callback->method=p_method;
|
|
fi_callback->udata=p_udata;
|
|
}
|
|
|
|
}
|
|
|
|
BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
|
|
|
|
|
|
mode=PhysicsServer::BODY_MODE_RIGID;
|
|
active=true;
|
|
|
|
mass=1;
|
|
//_inv_inertia=Transform();
|
|
_inv_mass=1;
|
|
bounce=0;
|
|
friction=1;
|
|
omit_force_integration=false;
|
|
//applied_torque=0;
|
|
island_step=0;
|
|
island_next=NULL;
|
|
island_list_next=NULL;
|
|
first_time_kinematic=false;
|
|
first_integration=false;
|
|
_set_static(false);
|
|
|
|
contact_count=0;
|
|
gravity_scale=1.0;
|
|
|
|
area_angular_damp=0;
|
|
area_linear_damp=0;
|
|
|
|
still_time=0;
|
|
continuous_cd=false;
|
|
can_sleep=false;
|
|
fi_callback=NULL;
|
|
axis_lock=PhysicsServer::BODY_AXIS_LOCK_DISABLED;
|
|
|
|
}
|
|
|
|
BodySW::~BodySW() {
|
|
|
|
if (fi_callback)
|
|
memdelete(fi_callback);
|
|
}
|
|
|
|
PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;
|
|
|
|
PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {
|
|
|
|
return body->get_space()->get_direct_state();
|
|
}
|