Skip to content

Commit

Permalink
Dynamic BVH broadphase in 2D & 3D Godot Physics
Browse files Browse the repository at this point in the history 
Port lawnjelly's dynamic BVH implementation from 3.x to be used in
both 2D and 3D broadphases.

Removed alternative broadphase implementations which are not meant to be
used anymore since they are much slower.

Includes changes in Rect2, Vector2, Vector3 that help with the template
implementation of the dynamic BVH by uniformizing the interface between
2D and 3D math.

Co-authored-by: lawnjelly <[email protected]>
  • Loading branch information
@pouleyKetchoupp @lawnjelly
pouleyKetchoupp and lawnjelly committed May 10, 2021
1 parent 3477379 commit 3877ed7
Show file tree
Hide file tree
Showing 33 changed files with 3,735 additions and 1,538 deletions.
695 changes: 695 additions & 0 deletions core/math/bvh.h
View file

Large diffs are not rendered by default.

276 changes: 276 additions & 0 deletions core/math/bvh_abb.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,276 @@
/*************************************************************************/
/* bvh_abb.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/

#ifndef BVH_ABB_H
#define BVH_ABB_H

// special optimized version of axis aligned bounding box
template <class Bounds = AABB, class Point = Vector3>
struct BVH_ABB {
struct ConvexHull {
// convex hulls (optional)
const Plane *planes;
int num_planes;
const Vector3 *points;
int num_points;
};

struct Segment {
Point from;
Point to;
};

enum IntersectResult {
IR_MISS = 0,
IR_PARTIAL,
IR_FULL,
};

// we store mins with a negative value in order to test them with SIMD
Point min;
Point neg_max;

bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); }
bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; }

void set(const Point &_min, const Point &_max) {
min = _min;
neg_max = -_max;
}

// to and from standard AABB
void from(const Bounds &p_aabb) {
min = p_aabb.position;
neg_max = -(p_aabb.position + p_aabb.size);
}

void to(Bounds &r_aabb) const {
r_aabb.position = min;
r_aabb.size = calculate_size();
}

void merge(const BVH_ABB &p_o) {
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]);
min[axis] = MIN(min[axis], p_o.min[axis]);
}
}

Point calculate_size() const {
return -neg_max - min;
}

Point calculate_centre() const {
return Point((calculate_size() * 0.5) + min);
}

real_t get_proximity_to(const BVH_ABB &p_b) const {
const Point d = (min - neg_max) - (p_b.min - p_b.neg_max);
real_t proximity = 0.0;
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
proximity += Math::abs(d[axis]);
}
return proximity;
}

int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const {
return (get_proximity_to(p_a) < get_proximity_to(p_b) ? 0 : 1);
}

uint32_t find_cutting_planes(const BVH_ABB::ConvexHull &p_hull, uint32_t *p_plane_ids) const {
uint32_t count = 0;

for (int n = 0; n < p_hull.num_planes; n++) {
const Plane &p = p_hull.planes[n];
if (intersects_plane(p)) {
p_plane_ids[count++] = n;
}
}

return count;
}

bool intersects_plane(const Plane &p_p) const {
Vector3 size = calculate_size();
Vector3 half_extents = size * 0.5;
Vector3 ofs = min + half_extents;

// forward side of plane?
Vector3 point_offset(
(p_p.normal.x < 0) ? -half_extents.x : half_extents.x,
(p_p.normal.y < 0) ? -half_extents.y : half_extents.y,
(p_p.normal.z < 0) ? -half_extents.z : half_extents.z);
Vector3 point = point_offset + ofs;

if (!p_p.is_point_over(point)) {
return false;
}

point = -point_offset + ofs;
if (p_p.is_point_over(point)) {
return false;
}

return true;
}

bool intersects_convex_optimized(const ConvexHull &p_hull, const uint32_t *p_plane_ids, uint32_t p_num_planes) const {
Vector3 size = calculate_size();
Vector3 half_extents = size * 0.5;
Vector3 ofs = min + half_extents;

for (unsigned int i = 0; i < p_num_planes; i++) {
const Plane &p = p_hull.planes[p_plane_ids[i]];
Vector3 point(
(p.normal.x > 0) ? -half_extents.x : half_extents.x,
(p.normal.y > 0) ? -half_extents.y : half_extents.y,
(p.normal.z > 0) ? -half_extents.z : half_extents.z);
point += ofs;
if (p.is_point_over(point)) {
return false;
}
}

return true;
}

bool intersects_convex_partial(const ConvexHull &p_hull) const {
Bounds bb;
to(bb);
return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points);
}

IntersectResult intersects_convex(const ConvexHull &p_hull) const {
if (intersects_convex_partial(p_hull)) {
// fully within? very important for tree checks
if (is_within_convex(p_hull)) {
return IR_FULL;
}

return IR_PARTIAL;
}

return IR_MISS;
}

bool is_within_convex(const ConvexHull &p_hull) const {
// use half extents routine
Bounds bb;
to(bb);
return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes);
}

bool is_point_within_hull(const ConvexHull &p_hull, const Vector3 &p_pt) const {
for (int n = 0; n < p_hull.num_planes; n++) {
if (p_hull.planes[n].distance_to(p_pt) > 0.0f) {
return false;
}
}
return true;
}

bool intersects_segment(const Segment &p_s) const {
Bounds bb;
to(bb);
return bb.intersects_segment(p_s.from, p_s.to);
}

bool intersects_point(const Point &p_pt) const {
if (_any_lessthan(-p_pt, neg_max)) {
return false;
}
if (_any_lessthan(p_pt, min)) {
return false;
}
return true;
}

bool intersects(const BVH_ABB &p_o) const {
if (_any_morethan(p_o.min, -neg_max)) {
return false;
}
if (_any_morethan(min, -p_o.neg_max)) {
return false;
}
return true;
}

bool is_other_within(const BVH_ABB &p_o) const {
if (_any_lessthan(p_o.neg_max, neg_max)) {
return false;
}
if (_any_lessthan(p_o.min, min)) {
return false;
}
return true;
}

void grow(const Point &p_change) {
neg_max -= p_change;
min -= p_change;
}

void expand(real_t p_change) {
Point change;
change.set_all(p_change);
grow(change);
}

// Actually surface area metric.
float get_area() const {
Point d = calculate_size();
return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
}

void set_to_max_opposite_extents() {
neg_max.set_all(FLT_MAX);
min = neg_max;
}

bool _any_morethan(const Point &p_a, const Point &p_b) const {
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
if (p_a[axis] > p_b[axis]) {
return true;
}
}
return false;
}

bool _any_lessthan(const Point &p_a, const Point &p_b) const {
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
if (p_a[axis] < p_b[axis]) {
return true;
}
}
return false;
}
};

#endif // BVH_ABB_H
Loading

0 comments on commit 3877ed7

Please sign in to comment.