asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

[3.3] Fix some angular velocity calculations

Browse source 
The angular velocity estimate for kinematic bodies was calculated
incorrectly. Also, fixes its use in some kinematic/rigid collision
calculations.

3.3 version of #47130
This commit is contained in:
e344fde6bf 2021-03-19 20:12:29 +10:00
parent f50c8062dd
commit 9671f8ff4b
2 changed files with 14 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
servers/physics/body_sw.cpp
View file

@ -517,22 +517,19 @@ void BodySW::integrate_forces(real_t p_step) {
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;
motion = new_transform.origin - get_transform().origin;
do_motion = true;
linear_velocity = motion / p_step;
//compute a FAKE angular velocity, not so easy
Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed();
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;
angular_velocity = axis * (angle / p_step);
} else {
if (!omit_force_integration && !first_integration) {
//overridden by direct state query

18
servers/physics/space_sw.cpp
View file

@ -317,7 +317,8 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform
best_first = false;
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
const BodySW *body = static_cast<const BodySW *>(col_obj);
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass());
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
}
}
}
@ -451,8 +452,8 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
if (rcd.best_object->get_type() == CollisionObjectSW::TYPE_BODY) {
const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
r_info->linear_velocity = body->get_linear_velocity() +
(body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass());
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
} else {
r_info->linear_velocity = Vector3();
@ -664,9 +665,8 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
BodySW *body = (BodySW *)col_obj;
Vector3 rel_vec = b - body->get_transform().get_origin();
//result.collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rel_vec); // * mPos);
Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass());
result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
}
}
}
@ -999,9 +999,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
//Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin();
// r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass());
r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
r_result->motion = safe * p_motion;
r_result->remainder = p_motion - safe * p_motion;