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

Analytic collision normals

This commit is contained in:
Peter Eastman 2023-01-14 18:39:23 -08:00
parent a51ca2beaf
commit 31c2a24893
9 changed files with 67 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
servers/physics_3d/gjk_epa.cpp
View File

@ -1011,9 +1011,11 @@ bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transfor
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
if (p_result_callback) {
if (p_swap) {
p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, p_userdata);
Vector3 normal = (res.witnesses[1] - res.witnesses[0]).normalized();
p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, normal, p_userdata);
} else {
p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, p_userdata);
Vector3 normal = (res.witnesses[0] - res.witnesses[1]).normalized();
p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, normal, p_userdata);
}
}
return true;

14
servers/physics_3d/godot_body_pair_3d.cpp
View File

@ -38,12 +38,12 @@
#define MIN_VELOCITY 0.0001
#define MAX_BIAS_ROTATION (Math_PI / 8)
void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata);
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
}
void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
@ -577,12 +577,12 @@ GodotBodyPair3D::~GodotBodyPair3D() {
B->remove_constraint(this);
}
void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata);
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
}
void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
Vector3 local_A = body->get_inv_transform().xform(p_point_A);
Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
@ -591,7 +591,7 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
contact.index_B = p_index_B;
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
contact.normal = (normal.dot((p_point_A - p_point_B)) < 0 ? -normal : normal);
contact.used = true;
// Attempt to determine if the contact will be reused.

8
servers/physics_3d/godot_body_pair_3d.h
View File

@ -97,9 +97,9 @@ class GodotBodyPair3D : public GodotBodyContact3D {
Contact contacts[MAX_CONTACTS];
int contact_count = 0;
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
void validate_contacts();
bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B);
@ -126,9 +126,9 @@ class GodotBodySoftBodyPair3D : public GodotBodyContact3D {
LocalVector<Contact> contacts;
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
void validate_contacts();

18
servers/physics_3d/godot_collision_solver_3d.cpp
View File

@ -81,9 +81,11 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(supports[i], 0, support_A, 0, p_userdata);
Vector3 normal = (support_A - supports[i]).normalized();
p_result_callback(supports[i], 0, support_A, 0, normal, p_userdata);
} else {
p_result_callback(support_A, 0, supports[i], 0, p_userdata);
Vector3 normal = (supports[i] - support_A).normalized();
p_result_callback(support_A, 0, supports[i], 0, normal, p_userdata);
}
}
}
@ -126,9 +128,11 @@ bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A,
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(support_B, 0, support_A, 0, p_userdata);
Vector3 normal = (support_B - support_A).normalized();
p_result_callback(support_B, 0, support_A, 0, normal, p_userdata);
} else {
p_result_callback(support_A, 0, support_B, 0, p_userdata);
Vector3 normal = (support_A - support_B).normalized();
p_result_callback(support_A, 0, support_B, 0, normal, p_userdata);
}
}
return true;
@ -142,7 +146,7 @@ struct _SoftBodyContactCollisionInfo {
int contact_count = 0;
};
void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
_SoftBodyContactCollisionInfo &cinfo = *(static_cast<_SoftBodyContactCollisionInfo *>(p_userdata));
++cinfo.contact_count;
@ -152,9 +156,9 @@ void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A
}
if (cinfo.swap_result) {
cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, -normal, cinfo.userdata);
} else {
cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata);
cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, normal, cinfo.userdata);
}
}

4
servers/physics_3d/godot_collision_solver_3d.h
View File

@ -35,11 +35,11 @@
class GodotCollisionSolver3D {
public:
typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
private:
static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);

59
servers/physics_3d/godot_collision_solver_3d_sat.cpp
View File

@ -75,11 +75,13 @@ struct _CollectorCallback {
Vector3 normal;
Vector3 *prev_axis = nullptr;
_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B, Vector3 p_normal) {
if (p_normal.dot(p_point_B - p_point_A) < 0)
p_normal = -p_normal;
if (swap) {
callback(p_point_B, 0, p_point_A, 0, userdata);
callback(p_point_B, 0, p_point_A, 0, -p_normal, userdata);
} else {
callback(p_point_A, 0, p_point_B, 0, userdata);
callback(p_point_A, 0, p_point_B, 0, p_normal, userdata);
}
}
};
@ -92,7 +94,7 @@ static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_poin
ERR_FAIL_COND(p_point_count_B != 1);
#endif
p_callback->call(*p_points_A, *p_points_B);
p_callback->call(*p_points_A, *p_points_B, p_callback->normal);
}
static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@ -102,7 +104,7 @@ static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point
#endif
Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
p_callback->call(*p_points_A, closest_B);
p_callback->call(*p_points_A, closest_B, p_callback->normal);
}
static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@ -111,9 +113,9 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point
ERR_FAIL_COND(p_point_count_B < 3);
#endif
Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A);
p_callback->call(*p_points_A, closest_B);
Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
Vector3 closest_B = plane.project(*p_points_A);
p_callback->call(*p_points_A, closest_B, plane.get_normal());
}
static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@ -122,9 +124,9 @@ static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_poi
ERR_FAIL_COND(p_point_count_B != 3);
#endif
Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A);
p_callback->call(*p_points_A, closest_B);
Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
Vector3 closest_B = plane.project(*p_points_A);
p_callback->call(*p_points_A, closest_B, plane.get_normal());
}
static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@ -154,8 +156,8 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
sa.sort(dvec, 4);
//use the middle ones as contacts
p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]);
p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]);
p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1], p_callback->normal);
p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2], p_callback->normal);
return;
}
@ -170,7 +172,14 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
Vector3 closest_A = p_points_A[0] + rel_A * d;
Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
p_callback->call(closest_A, closest_B);
// The normal should be perpendicular to both edges.
Vector3 normal = rel_A.cross(rel_B);
real_t normal_len = normal.length();
if (normal_len > 1e-3)
normal /= normal_len;
else
normal = p_callback->normal;
p_callback->call(closest_A, closest_B, normal);
}
static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@ -267,7 +276,7 @@ static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_poin
continue;
}
p_callback->call(contact_point_A, closest_B);
p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
}
}
@ -352,7 +361,7 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
continue;
}
p_callback->call(clipbuf_src[i], closest_B);
p_callback->call(clipbuf_src[i], closest_B, plane_B.get_normal());
}
}
@ -431,7 +440,7 @@ static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_poin
continue;
}
p_callback->call(contact_point_A, closest_B);
p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
}
}
@ -534,7 +543,7 @@ static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_po
continue;
}
p_callback->call(contact_point_A, closest_B);
p_callback->call(contact_point_A, closest_B, circle_B_plane.get_normal());
}
}
@ -678,7 +687,7 @@ public:
return true;
}
static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata;
Vector3 axis = (p_point_B - p_point_A);
real_t depth = axis.length();
@ -802,11 +811,11 @@ static void analytic_sphere_collision(const Vector3 &p_origin_a, real_t p_radius
if (p_radius_a < p_radius_b) {
Vector3 point_a = p_origin_a - b_to_a * p_radius_a;
Vector3 point_b = point_a + b_to_a * overlap;
p_collector->call(point_a, point_b); // Consider adding b_to_a vector
p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
} else {
Vector3 point_b = p_origin_b + b_to_a * p_radius_b;
Vector3 point_a = point_b - b_to_a * overlap;
p_collector->call(point_a, point_b); // Consider adding b_to_a vector
p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
}
}
@ -859,8 +868,8 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
axis = delta / length;
}
Vector3 point_a = p_transform_a.origin + (sphere_A->get_radius() + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest + p_margin_b * axis : nearest);
p_collector->call(point_a, point_b);
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>
@ -926,8 +935,8 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
axis = delta / length;
}
Vector3 point_a = p_transform_a.origin + (p_radius_a + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest + p_margin_b * axis : nearest);
p_collector->call(point_a, point_b);
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>

2
servers/physics_3d/godot_physics_server_3d.cpp
View File

@ -1737,7 +1737,7 @@ void GodotPhysicsServer3D::_update_shapes() {
}
}
void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata);
if (cbk->max == 0) {

2
servers/physics_3d/godot_physics_server_3d.h
View File

@ -77,7 +77,7 @@ public:
Vector3 *ptr = nullptr;
};
static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
virtual RID world_boundary_shape_create() override;
virtual RID separation_ray_shape_create() override;

6
servers/physics_3d/godot_space_3d.cpp
View File

@ -445,7 +445,7 @@ struct _RestCallbackData {
_RestResultData *other_results = nullptr;
};
static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
_RestCallbackData *rd = static_cast<_RestCallbackData *>(p_userdata);
Vector3 contact_rel = p_point_B - p_point_A;
@ -480,7 +480,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
// Keep this result as separate result.
result.len = len;
result.contact = p_point_B;
result.normal = contact_rel / len;
result.normal = normal;
result.object = rd->object;
result.shape = rd->shape;
result.local_shape = rd->local_shape;
@ -499,7 +499,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
rd->best_result.len = len;
rd->best_result.contact = p_point_B;
rd->best_result.normal = contact_rel / len;
rd->best_result.normal = normal;
rd->best_result.object = rd->object;
rd->best_result.shape = rd->shape;
rd->best_result.local_shape = rd->local_shape;