You've already forked godot
mirror of
https://github.com/godotengine/godot.git
synced 2025-11-08 12:40:44 +00:00
Lots of work on Audio & Physics engine:
-Added new 3D stream player node -Added ability for Area to capture sound from streams -Added small features in physics to be able to properly guess distance to areas for sound -Fixed 3D CollisionObject so shapes are added the same as in 2D, directly from children -Fixed KinematicBody API to make it the same as 2D.
This commit is contained in:
Juan Linietsky
@@ -45,6 +45,50 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object,
|
||||
return (1 << body->get_mode()) & p_type_mask;
|
||||
}
|
||||
|
||||
int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
|
||||
|
||||
ERR_FAIL_COND_V(space->locked, false);
|
||||
int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
|
||||
int cc = 0;
|
||||
|
||||
//Transform ai = p_xform.affine_inverse();
|
||||
|
||||
for (int i = 0; i < amount; i++) {
|
||||
|
||||
if (cc >= p_result_max)
|
||||
break;
|
||||
|
||||
if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
|
||||
continue;
|
||||
|
||||
//area can't be picked by ray (default)
|
||||
|
||||
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
|
||||
continue;
|
||||
|
||||
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
|
||||
int shape_idx = space->intersection_query_subindex_results[i];
|
||||
|
||||
Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
||||
inv_xform.affine_invert();
|
||||
|
||||
if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point)))
|
||||
continue;
|
||||
|
||||
r_results[cc].collider_id = col_obj->get_instance_id();
|
||||
if (r_results[cc].collider_id != 0)
|
||||
r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
|
||||
else
|
||||
r_results[cc].collider = NULL;
|
||||
r_results[cc].rid = col_obj->get_self();
|
||||
r_results[cc].shape = shape_idx;
|
||||
|
||||
cc++;
|
||||
}
|
||||
|
||||
return cc;
|
||||
}
|
||||
|
||||
bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) {
|
||||
|
||||
ERR_FAIL_COND_V(space->locked, false);
|
||||
@@ -428,6 +472,48 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
|
||||
|
||||
CollisionObjectSW *obj = NULL;
|
||||
obj = PhysicsServerSW::singleton->area_owner.getornull(p_object);
|
||||
if (!obj) {
|
||||
obj = PhysicsServerSW::singleton->body_owner.getornull(p_object);
|
||||
}
|
||||
ERR_FAIL_COND_V(!obj, Vector3());
|
||||
|
||||
ERR_FAIL_COND_V(obj->get_space() != space, Vector3());
|
||||
|
||||
float min_distance = 1e20;
|
||||
Vector3 min_point;
|
||||
|
||||
bool shapes_found = false;
|
||||
|
||||
for (int i = 0; i < obj->get_shape_count(); i++) {
|
||||
|
||||
if (obj->is_shape_set_as_disabled(i))
|
||||
continue;
|
||||
|
||||
Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i);
|
||||
ShapeSW *shape = obj->get_shape(i);
|
||||
|
||||
Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point));
|
||||
point = shape_xform.xform(point);
|
||||
|
||||
float dist = point.distance_to(p_point);
|
||||
if (dist < min_distance) {
|
||||
min_distance = dist;
|
||||
min_point = point;
|
||||
}
|
||||
shapes_found = true;
|
||||
}
|
||||
|
||||
if (!shapes_found) {
|
||||
return obj->get_transform().origin; //no shapes found, use distance to origin.
|
||||
} else {
|
||||
return min_point;
|
||||
}
|
||||
}
|
||||
|
||||
PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
|
||||
|
||||
space = NULL;
|
||||
@@ -435,6 +521,337 @@ PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) {
|
||||
|
||||
int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
|
||||
|
||||
for (int i = 0; i < amount; i++) {
|
||||
|
||||
bool keep = true;
|
||||
|
||||
if (intersection_query_results[i] == p_body)
|
||||
keep = false;
|
||||
else if (intersection_query_results[i]->get_type() == CollisionObjectSW::TYPE_AREA)
|
||||
keep = false;
|
||||
else if ((static_cast<BodySW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0)
|
||||
keep = false;
|
||||
else if (static_cast<BodySW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
|
||||
keep = false;
|
||||
else if (static_cast<BodySW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i]))
|
||||
keep = false;
|
||||
|
||||
if (!keep) {
|
||||
|
||||
if (i < amount - 1) {
|
||||
SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
|
||||
SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
|
||||
}
|
||||
|
||||
amount--;
|
||||
i--;
|
||||
}
|
||||
}
|
||||
|
||||
return amount;
|
||||
}
|
||||
|
||||
bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) {
|
||||
|
||||
//give me back regular physics engine logic
|
||||
//this is madness
|
||||
//and most people using this function will think
|
||||
//what it does is simpler than using physics
|
||||
//this took about a week to get right..
|
||||
//but is it right? who knows at this point..
|
||||
|
||||
if (r_result) {
|
||||
r_result->collider_id = 0;
|
||||
r_result->collider_shape = 0;
|
||||
}
|
||||
Rect3 body_aabb;
|
||||
|
||||
for (int i = 0; i < p_body->get_shape_count(); i++) {
|
||||
|
||||
if (i == 0)
|
||||
body_aabb = p_body->get_shape_aabb(i);
|
||||
else
|
||||
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
|
||||
}
|
||||
|
||||
// Undo the currently transform the physics server is aware of and apply the provided one
|
||||
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
|
||||
body_aabb = body_aabb.grow(p_margin);
|
||||
|
||||
Transform body_transform = p_from;
|
||||
|
||||
{
|
||||
//STEP 1, FREE BODY IF STUCK
|
||||
|
||||
const int max_results = 32;
|
||||
int recover_attempts = 4;
|
||||
Vector3 sr[max_results * 2];
|
||||
|
||||
do {
|
||||
|
||||
PhysicsServerSW::CollCbkData cbk;
|
||||
cbk.max = max_results;
|
||||
cbk.amount = 0;
|
||||
cbk.ptr = sr;
|
||||
|
||||
CollisionSolverSW::CallbackResult cbkres = NULL;
|
||||
|
||||
PhysicsServerSW::CollCbkData *cbkptr = NULL;
|
||||
cbkptr = &cbk;
|
||||
cbkres = PhysicsServerSW::_shape_col_cbk;
|
||||
|
||||
bool collided = false;
|
||||
|
||||
int amount = _cull_aabb_for_body(p_body, body_aabb);
|
||||
|
||||
for (int j = 0; j < p_body->get_shape_count(); j++) {
|
||||
if (p_body->is_shape_set_as_disabled(j))
|
||||
continue;
|
||||
|
||||
Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
|
||||
ShapeSW *body_shape = p_body->get_shape(j);
|
||||
for (int i = 0; i < amount; i++) {
|
||||
|
||||
const CollisionObjectSW *col_obj = intersection_query_results[i];
|
||||
int shape_idx = intersection_query_subindex_results[i];
|
||||
|
||||
if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
|
||||
collided = cbk.amount > 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!collided) {
|
||||
break;
|
||||
}
|
||||
|
||||
Vector3 recover_motion;
|
||||
|
||||
for (int i = 0; i < cbk.amount; i++) {
|
||||
|
||||
Vector3 a = sr[i * 2 + 0];
|
||||
Vector3 b = sr[i * 2 + 1];
|
||||
|
||||
#if 0
|
||||
Vector3 rel = b-a;
|
||||
real_t d = rel.length();
|
||||
if (d==0)
|
||||
continue;
|
||||
|
||||
Vector3 n = rel/d;
|
||||
real_t traveled = n.dot(recover_motion);
|
||||
a+=n*traveled;
|
||||
|
||||
real_t d = a.distance_to(b);
|
||||
if (d<margin)
|
||||
continue;
|
||||
#endif
|
||||
recover_motion += (b - a) * 0.4;
|
||||
}
|
||||
|
||||
if (recover_motion == Vector3()) {
|
||||
collided = false;
|
||||
break;
|
||||
}
|
||||
|
||||
body_transform.origin += recover_motion;
|
||||
body_aabb.position += recover_motion;
|
||||
|
||||
recover_attempts--;
|
||||
|
||||
} while (recover_attempts);
|
||||
}
|
||||
|
||||
real_t safe = 1.0;
|
||||
real_t unsafe = 1.0;
|
||||
int best_shape = -1;
|
||||
|
||||
{
|
||||
// STEP 2 ATTEMPT MOTION
|
||||
|
||||
Rect3 motion_aabb = body_aabb;
|
||||
motion_aabb.position += p_motion;
|
||||
motion_aabb = motion_aabb.merge(body_aabb);
|
||||
|
||||
int amount = _cull_aabb_for_body(p_body, motion_aabb);
|
||||
|
||||
for (int j = 0; j < p_body->get_shape_count(); j++) {
|
||||
|
||||
if (p_body->is_shape_set_as_disabled(j))
|
||||
continue;
|
||||
|
||||
Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
|
||||
ShapeSW *body_shape = p_body->get_shape(j);
|
||||
|
||||
Transform body_shape_xform_inv = body_shape_xform.affine_inverse();
|
||||
MotionShapeSW mshape;
|
||||
mshape.shape = body_shape;
|
||||
mshape.motion = body_shape_xform_inv.basis.xform(p_motion);
|
||||
|
||||
bool stuck = false;
|
||||
|
||||
real_t best_safe = 1;
|
||||
real_t best_unsafe = 1;
|
||||
|
||||
for (int i = 0; i < amount; i++) {
|
||||
|
||||
const CollisionObjectSW *col_obj = intersection_query_results[i];
|
||||
int shape_idx = intersection_query_subindex_results[i];
|
||||
|
||||
//test initial overlap, does it collide if going all the way?
|
||||
Vector3 point_A, point_B;
|
||||
Vector3 sep_axis = p_motion.normalized();
|
||||
|
||||
Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
||||
//test initial overlap, does it collide if going all the way?
|
||||
if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
|
||||
//print_line("failed motion cast (no collision)");
|
||||
continue;
|
||||
}
|
||||
sep_axis = p_motion.normalized();
|
||||
|
||||
if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
|
||||
//print_line("failed motion cast (no collision)");
|
||||
stuck = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//just do kinematic solving
|
||||
real_t low = 0;
|
||||
real_t hi = 1;
|
||||
Vector3 mnormal = p_motion.normalized();
|
||||
|
||||
for (int i = 0; i < 8; i++) { //steps should be customizable..
|
||||
|
||||
real_t ofs = (low + hi) * 0.5;
|
||||
|
||||
Vector3 sep = mnormal; //important optimization for this to work fast enough
|
||||
|
||||
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
|
||||
|
||||
Vector3 lA, lB;
|
||||
|
||||
bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
|
||||
|
||||
if (collided) {
|
||||
|
||||
//print_line(itos(i)+": "+rtos(ofs));
|
||||
hi = ofs;
|
||||
} else {
|
||||
|
||||
point_A = lA;
|
||||
point_B = lB;
|
||||
low = ofs;
|
||||
}
|
||||
}
|
||||
|
||||
if (low < best_safe) {
|
||||
best_safe = low;
|
||||
best_unsafe = hi;
|
||||
}
|
||||
}
|
||||
|
||||
if (stuck) {
|
||||
|
||||
safe = 0;
|
||||
unsafe = 0;
|
||||
best_shape = j; //sadly it's the best
|
||||
break;
|
||||
}
|
||||
if (best_safe == 1.0) {
|
||||
continue;
|
||||
}
|
||||
if (best_safe < safe) {
|
||||
|
||||
safe = best_safe;
|
||||
unsafe = best_unsafe;
|
||||
best_shape = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool collided = false;
|
||||
if (safe >= 1) {
|
||||
//not collided
|
||||
collided = false;
|
||||
if (r_result) {
|
||||
|
||||
r_result->motion = p_motion;
|
||||
r_result->remainder = Vector3();
|
||||
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
//it collided, let's get the rest info in unsafe advance
|
||||
Transform ugt = body_transform;
|
||||
ugt.origin += p_motion * unsafe;
|
||||
|
||||
_RestCallbackData rcd;
|
||||
rcd.best_len = 0;
|
||||
rcd.best_object = NULL;
|
||||
rcd.best_shape = 0;
|
||||
|
||||
Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
|
||||
ShapeSW *body_shape = p_body->get_shape(best_shape);
|
||||
|
||||
body_aabb.position += p_motion * unsafe;
|
||||
|
||||
int amount = _cull_aabb_for_body(p_body, body_aabb);
|
||||
|
||||
for (int i = 0; i < amount; i++) {
|
||||
|
||||
const CollisionObjectSW *col_obj = intersection_query_results[i];
|
||||
int shape_idx = intersection_query_subindex_results[i];
|
||||
|
||||
rcd.object = col_obj;
|
||||
rcd.shape = shape_idx;
|
||||
bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin);
|
||||
if (!sc)
|
||||
continue;
|
||||
}
|
||||
|
||||
if (rcd.best_len != 0) {
|
||||
|
||||
if (r_result) {
|
||||
r_result->collider = rcd.best_object->get_self();
|
||||
r_result->collider_id = rcd.best_object->get_instance_id();
|
||||
r_result->collider_shape = rcd.best_shape;
|
||||
r_result->collision_local_shape = best_shape;
|
||||
r_result->collision_normal = rcd.best_normal;
|
||||
r_result->collision_point = rcd.best_contact;
|
||||
//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);
|
||||
|
||||
r_result->motion = safe * p_motion;
|
||||
r_result->remainder = p_motion - safe * p_motion;
|
||||
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
|
||||
}
|
||||
|
||||
collided = true;
|
||||
} else {
|
||||
if (r_result) {
|
||||
|
||||
r_result->motion = p_motion;
|
||||
r_result->remainder = Vector3();
|
||||
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
|
||||
}
|
||||
|
||||
collided = false;
|
||||
}
|
||||
}
|
||||
|
||||
return collided;
|
||||
}
|
||||
|
||||
void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
|
||||
|
||||
CollisionObjectSW::Type type_A = A->get_type();
|
||||
|
||||
Reference in New Issue
Block a user