Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Cylinder support in Godot Physics 3D #45854

Merged
merged 1 commit into from
Feb 10, 2021
Merged

Cylinder support in Godot Physics 3D #45854

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions COPYRIGHT.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,11 @@ Copyright: 2008-2016, The Android Open Source Project
2002, Google, Inc.
License: Apache-2.0

Files: ./servers/physics_3d/collision_solver_3d_sat.cpp
Comment: Open Dynamics Engine
Copyright: 2001-2003, Russell L. Smith, Alen Ladavac, Nguyen Binh
License: BSD-3-clause

Files: ./servers/physics_3d/gjk_epa.cpp
./servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
./servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
Expand Down
33 changes: 21 additions & 12 deletions core/math/geometry_3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,27 +252,34 @@ class Geometry3D {
return true;
}

static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) {
static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr, int p_cylinder_axis = 2) {
Vector3 rel = (p_to - p_from);
real_t rel_l = rel.length();
if (rel_l < CMP_EPSILON) {
return false; // Both points are the same.
}

ERR_FAIL_COND_V(p_cylinder_axis < 0, false);
ERR_FAIL_COND_V(p_cylinder_axis > 2, false);
Vector3 cylinder_axis;
cylinder_axis[p_cylinder_axis] = 1.0;

// First check if they are parallel.
Vector3 normal = (rel / rel_l);
Vector3 crs = normal.cross(Vector3(0, 0, 1));
Vector3 crs = normal.cross(cylinder_axis);
real_t crs_l = crs.length();

Vector3 z_dir;
Vector3 axis_dir;

if (crs_l < CMP_EPSILON) {
z_dir = Vector3(1, 0, 0); // Any x/y vector OK.
Vector3 side_axis;
side_axis[(p_cylinder_axis + 1) % 3] = 1.0; // Any side axis OK.
axis_dir = side_axis;
} else {
z_dir = crs / crs_l;
axis_dir = crs / crs_l;
}

real_t dist = z_dir.dot(p_from);
real_t dist = axis_dir.dot(p_from);

if (dist >= p_radius) {
return false; // Too far away.
Expand All @@ -285,10 +292,10 @@ class Geometry3D {
}
Size2 size(Math::sqrt(w2), p_height * 0.5);

Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized();
Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized();

Vector2 from2D(x_dir.dot(p_from), p_from.z);
Vector2 to2D(x_dir.dot(p_to), p_to.z);
Vector2 from2D(side_dir.dot(p_from), p_from[p_cylinder_axis]);
Vector2 to2D(side_dir.dot(p_to), p_to[p_cylinder_axis]);

real_t min = 0, max = 1;

Expand Down Expand Up @@ -335,10 +342,12 @@ class Geometry3D {
Vector3 res_normal = result;

if (axis == 0) {
res_normal.z = 0;
res_normal[p_cylinder_axis] = 0;
} else {
res_normal.x = 0;
res_normal.y = 0;
int axis_side = (p_cylinder_axis + 1) % 3;
res_normal[axis_side] = 0;
axis_side = (axis_side + 1) % 3;
res_normal[axis_side] = 0;
}

res_normal.normalize();
Expand Down
Loading