godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix some angular velocity calculations 

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

This fixes #47029.
This commit is contained in:
e344fde6bf 2021-03-18 23:14:19 +10:00
parent ec3f220098
commit 1c123aaf2b
2 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
servers/physics_3d/body_3d_sw.cpp
View File

@ -501,20 +501,18 @@ void Body3DSW::integrate_forces(real_t p_step) {
if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
//compute motion, angular and etc. velocities from prev transform //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 //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; Vector3 axis;
real_t angle; real_t angle;
rot.get_axis_angle(axis, angle); rot.get_axis_angle(axis, angle);
axis.normalize(); axis.normalize();
angular_velocity = axis.normalized() * (angle / p_step); angular_velocity = axis * (angle / p_step);
motion = new_transform.origin - get_transform().origin;
do_motion = true;
} else { } else {
if (!omit_force_integration && !first_integration) { if (!omit_force_integration && !first_integration) {
//overridden by direct state query //overridden by direct state query

19
servers/physics_3d/space_3d_sw.cpp
View File

@ -332,7 +332,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
best_first = false; best_first = false;
if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) {
const Body3DSW *body = static_cast<const Body3DSW *>(col_obj); const Body3DSW *body = static_cast<const Body3DSW *>(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);
} }
} }
} }
@ -478,8 +479,8 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap
r_info->rid = rcd.best_object->get_self(); r_info->rid = rcd.best_object->get_self();
if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) { if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) {
const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object);
r_info->linear_velocity = body->get_linear_velocity() + Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass());
(body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
} else { } else {
r_info->linear_velocity = Vector3(); r_info->linear_velocity = Vector3();
@ -684,10 +685,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
//result.collider_metadata = col_obj->get_shape_metadata(shape_idx); //result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) {
Body3DSW *body = (Body3DSW *)col_obj; Body3DSW *body = (Body3DSW *)col_obj;
Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass());
Vector3 rel_vec = b - body->get_transform().get_origin(); result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
//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);
} }
} }
} }
@ -1009,9 +1008,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); const Body3DSW *body = static_cast<const Body3DSW *>(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(); 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(body->get_transform().origin - rcd.best_contact); // * mPos); r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
r_result->motion = safe * p_motion; r_result->motion = safe * p_motion;
r_result->remainder = p_motion - safe * p_motion; r_result->remainder = p_motion - safe * p_motion;