void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
// See the comment in get_scale() for further information.
Basis m = orthonormalized();
real_t det = m.determinant();
if (det < 0) {
// Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
m.scale(Vector3(-1, -1, -1));
}
m.get_axis_angle(p_axis, p_angle);
}
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;
real_t angle;
rot.get_axis_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) {
//overridden 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;
}