33b5c57199
- Renames PackedIntArray to PackedInt32Array. - Renames PackedFloatArray to PackedFloat32Array. - Adds PackedInt64Array and PackedFloat64Array. - Renames Variant::REAL to Variant::FLOAT for consistency. Packed arrays are for storing large amount of data and creating stuff like meshes, buffers. textures, etc. Forcing them to be 64 is a huge waste of memory. That said, many users requested the ability to have 64 bits packed arrays for their games, so this is just an optional added type. For Variant, the float datatype is always 64 bits, and exposed as `float`. We still have `real_t` which is the datatype that can change from 32 to 64 bits depending on a compile flag (not entirely working right now, but that's the idea). It affects math related datatypes and code only. Neither Variant nor PackedArray make use of real_t, which is only intended for math precision, so the term is removed from there to keep only float.
1243 lines
38 KiB
C++
1243 lines
38 KiB
C++
/*************************************************************************/
|
|
/* space_sw.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
|
|
/* Copyright (c) 2014-2020 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. */
|
|
/*************************************************************************/
|
|
|
|
#include "space_sw.h"
|
|
|
|
#include "collision_solver_sw.h"
|
|
#include "core/project_settings.h"
|
|
#include "physics_server_sw.h"
|
|
|
|
_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
|
|
|
|
if (!(p_object->get_collision_layer() & p_collision_mask)) {
|
|
return false;
|
|
}
|
|
|
|
if (p_object->get_type() == CollisionObjectSW::TYPE_AREA && !p_collide_with_areas)
|
|
return false;
|
|
|
|
if (p_object->get_type() == CollisionObjectSW::TYPE_BODY && !p_collide_with_bodies)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
|
|
|
|
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 (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
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.is_valid())
|
|
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_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) {
|
|
|
|
ERR_FAIL_COND_V(space->locked, false);
|
|
|
|
Vector3 begin, end;
|
|
Vector3 normal;
|
|
begin = p_from;
|
|
end = p_to;
|
|
normal = (end - begin).normalized();
|
|
|
|
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
|
|
|
|
//todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
|
|
|
|
bool collided = false;
|
|
Vector3 res_point, res_normal;
|
|
int res_shape;
|
|
const CollisionObjectSW *res_obj;
|
|
real_t min_d = 1e10;
|
|
|
|
for (int i = 0; i < amount; i++) {
|
|
|
|
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
continue;
|
|
|
|
if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable()))
|
|
continue;
|
|
|
|
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_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
|
|
|
|
Vector3 local_from = inv_xform.xform(begin);
|
|
Vector3 local_to = inv_xform.xform(end);
|
|
|
|
const ShapeSW *shape = col_obj->get_shape(shape_idx);
|
|
|
|
Vector3 shape_point, shape_normal;
|
|
|
|
if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
|
|
|
|
Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
|
shape_point = xform.xform(shape_point);
|
|
|
|
real_t ld = normal.dot(shape_point);
|
|
|
|
if (ld < min_d) {
|
|
|
|
min_d = ld;
|
|
res_point = shape_point;
|
|
res_normal = inv_xform.basis.xform_inv(shape_normal).normalized();
|
|
res_shape = shape_idx;
|
|
res_obj = col_obj;
|
|
collided = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!collided)
|
|
return false;
|
|
|
|
r_result.collider_id = res_obj->get_instance_id();
|
|
if (r_result.collider_id.is_valid())
|
|
r_result.collider = ObjectDB::get_instance(r_result.collider_id);
|
|
else
|
|
r_result.collider = NULL;
|
|
r_result.normal = res_normal;
|
|
r_result.position = res_point;
|
|
r_result.rid = res_obj->get_self();
|
|
r_result.shape = res_shape;
|
|
|
|
return true;
|
|
}
|
|
|
|
int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
|
|
|
|
if (p_result_max <= 0)
|
|
return 0;
|
|
|
|
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape);
|
|
ERR_FAIL_COND_V(!shape, 0);
|
|
|
|
AABB aabb = p_xform.xform(shape->get_aabb());
|
|
|
|
int amount = space->broadphase->cull_aabb(aabb, 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 (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
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];
|
|
|
|
if (!CollisionSolverSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL, NULL, NULL, p_margin, 0))
|
|
continue;
|
|
|
|
if (r_results) {
|
|
r_results[cc].collider_id = col_obj->get_instance_id();
|
|
if (r_results[cc].collider_id.is_valid())
|
|
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::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
|
|
|
|
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape);
|
|
ERR_FAIL_COND_V(!shape, false);
|
|
|
|
AABB aabb = p_xform.xform(shape->get_aabb());
|
|
aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion
|
|
aabb = aabb.grow(p_margin);
|
|
|
|
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
|
|
|
|
real_t best_safe = 1;
|
|
real_t best_unsafe = 1;
|
|
|
|
Transform xform_inv = p_xform.affine_inverse();
|
|
MotionShapeSW mshape;
|
|
mshape.shape = shape;
|
|
mshape.motion = xform_inv.basis.xform(p_motion);
|
|
|
|
bool best_first = true;
|
|
|
|
Vector3 closest_A, closest_B;
|
|
|
|
for (int i = 0; i < amount; i++) {
|
|
|
|
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
continue;
|
|
|
|
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
|
|
continue; //ignore excluded
|
|
|
|
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
|
|
int shape_idx = space->intersection_query_subindex_results[i];
|
|
|
|
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, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
|
|
continue;
|
|
}
|
|
|
|
//test initial overlap
|
|
sep_axis = p_motion.normalized();
|
|
|
|
if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
|
|
return false;
|
|
}
|
|
|
|
//just do kinematic solving
|
|
real_t low = 0;
|
|
real_t hi = 1;
|
|
Vector3 mnormal = p_motion.normalized();
|
|
|
|
for (int j = 0; j < 8; j++) { //steps should be customizable..
|
|
|
|
real_t ofs = (low + hi) * 0.5;
|
|
|
|
Vector3 sep = mnormal; //important optimization for this to work fast enough
|
|
|
|
mshape.motion = xform_inv.basis.xform(p_motion * ofs);
|
|
|
|
Vector3 lA, lB;
|
|
|
|
bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
|
|
|
|
if (collided) {
|
|
|
|
hi = ofs;
|
|
} else {
|
|
|
|
point_A = lA;
|
|
point_B = lB;
|
|
low = ofs;
|
|
}
|
|
}
|
|
|
|
if (low < best_safe) {
|
|
best_first = true; //force reset
|
|
best_safe = low;
|
|
best_unsafe = hi;
|
|
}
|
|
|
|
if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) {
|
|
closest_A = point_A;
|
|
closest_B = point_B;
|
|
r_info->collider_id = col_obj->get_instance_id();
|
|
r_info->rid = col_obj->get_self();
|
|
r_info->shape = shape_idx;
|
|
r_info->point = closest_B;
|
|
r_info->normal = (closest_A - closest_B).normalized();
|
|
best_first = false;
|
|
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
|
|
const BodySW *body = static_cast<const BodySW *>(col_obj);
|
|
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
|
|
}
|
|
}
|
|
}
|
|
|
|
p_closest_safe = best_safe;
|
|
p_closest_unsafe = best_unsafe;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
|
|
|
|
if (p_result_max <= 0)
|
|
return 0;
|
|
|
|
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape);
|
|
ERR_FAIL_COND_V(!shape, 0);
|
|
|
|
AABB aabb = p_shape_xform.xform(shape->get_aabb());
|
|
aabb = aabb.grow(p_margin);
|
|
|
|
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
|
|
|
|
bool collided = false;
|
|
r_result_count = 0;
|
|
|
|
PhysicsServerSW::CollCbkData cbk;
|
|
cbk.max = p_result_max;
|
|
cbk.amount = 0;
|
|
cbk.ptr = r_results;
|
|
CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk;
|
|
|
|
PhysicsServerSW::CollCbkData *cbkptr = &cbk;
|
|
|
|
for (int i = 0; i < amount; i++) {
|
|
|
|
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
continue;
|
|
|
|
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
|
|
int shape_idx = space->intersection_query_subindex_results[i];
|
|
|
|
if (p_exclude.has(col_obj->get_self())) {
|
|
continue;
|
|
}
|
|
|
|
if (CollisionSolverSW::solve_static(shape, p_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 = true;
|
|
}
|
|
}
|
|
|
|
r_result_count = cbk.amount;
|
|
|
|
return collided;
|
|
}
|
|
|
|
struct _RestCallbackData {
|
|
|
|
const CollisionObjectSW *object;
|
|
const CollisionObjectSW *best_object;
|
|
int shape;
|
|
int best_shape;
|
|
Vector3 best_contact;
|
|
Vector3 best_normal;
|
|
real_t best_len;
|
|
real_t min_allowed_depth;
|
|
};
|
|
|
|
static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
|
|
|
|
_RestCallbackData *rd = (_RestCallbackData *)p_userdata;
|
|
|
|
Vector3 contact_rel = p_point_B - p_point_A;
|
|
real_t len = contact_rel.length();
|
|
if (len < rd->min_allowed_depth)
|
|
return;
|
|
if (len <= rd->best_len)
|
|
return;
|
|
|
|
rd->best_len = len;
|
|
rd->best_contact = p_point_B;
|
|
rd->best_normal = contact_rel / len;
|
|
rd->best_object = rd->object;
|
|
rd->best_shape = rd->shape;
|
|
}
|
|
bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
|
|
|
|
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.getornull(p_shape);
|
|
ERR_FAIL_COND_V(!shape, 0);
|
|
|
|
AABB aabb = p_shape_xform.xform(shape->get_aabb());
|
|
aabb = aabb.grow(p_margin);
|
|
|
|
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
|
|
|
|
_RestCallbackData rcd;
|
|
rcd.best_len = 0;
|
|
rcd.best_object = NULL;
|
|
rcd.best_shape = 0;
|
|
rcd.min_allowed_depth = space->test_motion_min_contact_depth;
|
|
|
|
for (int i = 0; i < amount; i++) {
|
|
|
|
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas))
|
|
continue;
|
|
|
|
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
|
|
int shape_idx = space->intersection_query_subindex_results[i];
|
|
|
|
if (p_exclude.has(col_obj->get_self()))
|
|
continue;
|
|
|
|
rcd.object = col_obj;
|
|
rcd.shape = shape_idx;
|
|
bool sc = CollisionSolverSW::solve_static(shape, p_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 || !rcd.best_object)
|
|
return false;
|
|
|
|
r_info->collider_id = rcd.best_object->get_instance_id();
|
|
r_info->shape = rcd.best_shape;
|
|
r_info->normal = rcd.best_normal;
|
|
r_info->point = rcd.best_contact;
|
|
r_info->rid = rcd.best_object->get_self();
|
|
if (rcd.best_object->get_type() == CollisionObjectSW::TYPE_BODY) {
|
|
|
|
const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
|
|
r_info->linear_velocity = body->get_linear_velocity() +
|
|
(body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
|
|
|
|
} else {
|
|
r_info->linear_velocity = Vector3();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
|
|
|
|
CollisionObjectSW *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;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &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;
|
|
}
|
|
|
|
int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer::SeparationResult *r_results, int p_result_max, real_t p_margin) {
|
|
|
|
AABB body_aabb;
|
|
|
|
bool shapes_found = false;
|
|
|
|
for (int i = 0; i < p_body->get_shape_count(); i++) {
|
|
|
|
if (p_body->is_shape_set_as_disabled(i))
|
|
continue;
|
|
|
|
if (!shapes_found) {
|
|
body_aabb = p_body->get_shape_aabb(i);
|
|
shapes_found = true;
|
|
} else {
|
|
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
|
|
}
|
|
}
|
|
|
|
if (!shapes_found) {
|
|
return 0;
|
|
}
|
|
// Undo the currently transform the physics server is aware of and apply the provided one
|
|
body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb));
|
|
body_aabb = body_aabb.grow(p_margin);
|
|
|
|
Transform body_transform = p_transform;
|
|
|
|
for (int i = 0; i < p_result_max; i++) {
|
|
//reset results
|
|
r_results[i].collision_depth = 0;
|
|
}
|
|
|
|
int rays_found = 0;
|
|
|
|
{
|
|
// raycast AND separate
|
|
|
|
const int max_results = 32;
|
|
int recover_attempts = 4;
|
|
Vector3 sr[max_results * 2];
|
|
PhysicsServerSW::CollCbkData cbk;
|
|
cbk.max = max_results;
|
|
PhysicsServerSW::CollCbkData *cbkptr = &cbk;
|
|
CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk;
|
|
|
|
do {
|
|
|
|
Vector3 recover_motion;
|
|
|
|
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;
|
|
|
|
ShapeSW *body_shape = p_body->get_shape(j);
|
|
|
|
if (body_shape->get_type() != PhysicsServer::SHAPE_RAY)
|
|
continue;
|
|
|
|
Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
|
|
|
|
for (int i = 0; i < amount; i++) {
|
|
|
|
const CollisionObjectSW *col_obj = intersection_query_results[i];
|
|
int shape_idx = intersection_query_subindex_results[i];
|
|
|
|
cbk.amount = 0;
|
|
cbk.ptr = sr;
|
|
|
|
if (CollisionObjectSW::TYPE_BODY == col_obj->get_type()) {
|
|
const BodySW *b = static_cast<const BodySW *>(col_obj);
|
|
if (p_infinite_inertia && PhysicsServer::BODY_MODE_STATIC != b->get_mode() && PhysicsServer::BODY_MODE_KINEMATIC != b->get_mode()) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
ShapeSW *against_shape = col_obj->get_shape(shape_idx);
|
|
if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
|
|
if (cbk.amount > 0) {
|
|
collided = true;
|
|
}
|
|
|
|
int ray_index = -1; //reuse shape
|
|
for (int k = 0; k < rays_found; k++) {
|
|
if (r_results[k].collision_local_shape == j) {
|
|
ray_index = k;
|
|
}
|
|
}
|
|
|
|
if (ray_index == -1 && rays_found < p_result_max) {
|
|
ray_index = rays_found;
|
|
rays_found++;
|
|
}
|
|
|
|
if (ray_index != -1) {
|
|
PhysicsServer::SeparationResult &result = r_results[ray_index];
|
|
|
|
for (int k = 0; k < cbk.amount; k++) {
|
|
Vector3 a = sr[k * 2 + 0];
|
|
Vector3 b = sr[k * 2 + 1];
|
|
|
|
recover_motion += (b - a) * 0.4;
|
|
|
|
float depth = a.distance_to(b);
|
|
if (depth > result.collision_depth) {
|
|
|
|
result.collision_depth = depth;
|
|
result.collision_point = b;
|
|
result.collision_normal = (b - a).normalized();
|
|
result.collision_local_shape = j;
|
|
result.collider = col_obj->get_self();
|
|
result.collider_id = col_obj->get_instance_id();
|
|
result.collider_shape = shape_idx;
|
|
//result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
|
|
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
|
|
BodySW *body = (BodySW *)col_obj;
|
|
|
|
Vector3 rel_vec = b - body->get_transform().get_origin();
|
|
//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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!collided || recover_motion == Vector3()) {
|
|
break;
|
|
}
|
|
|
|
body_transform.origin += recover_motion;
|
|
body_aabb.position += recover_motion;
|
|
|
|
recover_attempts--;
|
|
} while (recover_attempts);
|
|
}
|
|
|
|
//optimize results (remove non colliding)
|
|
for (int i = 0; i < rays_found; i++) {
|
|
if (r_results[i].collision_depth == 0) {
|
|
rays_found--;
|
|
SWAP(r_results[i], r_results[rays_found]);
|
|
}
|
|
}
|
|
|
|
r_recover_motion = body_transform.origin - p_transform.origin;
|
|
return rays_found;
|
|
}
|
|
|
|
bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result, bool p_exclude_raycast_shapes) {
|
|
|
|
//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 = ObjectID();
|
|
r_result->collider_shape = 0;
|
|
}
|
|
AABB body_aabb;
|
|
bool shapes_found = false;
|
|
|
|
for (int i = 0; i < p_body->get_shape_count(); i++) {
|
|
|
|
if (p_body->is_shape_set_as_disabled(i))
|
|
continue;
|
|
|
|
if (!shapes_found) {
|
|
body_aabb = p_body->get_shape_aabb(i);
|
|
shapes_found = true;
|
|
} else {
|
|
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
|
|
}
|
|
}
|
|
|
|
if (!shapes_found) {
|
|
if (r_result) {
|
|
*r_result = PhysicsServer::MotionResult();
|
|
r_result->motion = p_motion;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
// 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;
|
|
|
|
PhysicsServerSW::CollCbkData *cbkptr = &cbk;
|
|
CollisionSolverSW::CallbackResult 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);
|
|
if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) {
|
|
continue;
|
|
}
|
|
|
|
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];
|
|
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
|
|
|
|
AABB 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);
|
|
|
|
if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::SHAPE_RAY) {
|
|
continue;
|
|
}
|
|
|
|
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)) {
|
|
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)) {
|
|
stuck = true;
|
|
break;
|
|
}
|
|
|
|
//just do kinematic solving
|
|
real_t low = 0;
|
|
real_t hi = 1;
|
|
Vector3 mnormal = p_motion.normalized();
|
|
|
|
for (int k = 0; k < 8; k++) { //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) {
|
|
|
|
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;
|
|
rcd.min_allowed_depth = test_motion_min_contact_depth;
|
|
|
|
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();
|
|
CollisionObjectSW::Type type_B = B->get_type();
|
|
if (type_A > type_B) {
|
|
|
|
SWAP(A, B);
|
|
SWAP(p_subindex_A, p_subindex_B);
|
|
SWAP(type_A, type_B);
|
|
}
|
|
|
|
SpaceSW *self = (SpaceSW *)p_self;
|
|
|
|
self->collision_pairs++;
|
|
|
|
if (type_A == CollisionObjectSW::TYPE_AREA) {
|
|
|
|
AreaSW *area = static_cast<AreaSW *>(A);
|
|
if (type_B == CollisionObjectSW::TYPE_AREA) {
|
|
|
|
AreaSW *area_b = static_cast<AreaSW *>(B);
|
|
Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A));
|
|
return area2_pair;
|
|
} else {
|
|
|
|
BodySW *body = static_cast<BodySW *>(B);
|
|
AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A));
|
|
return area_pair;
|
|
}
|
|
} else {
|
|
|
|
BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B));
|
|
return b;
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) {
|
|
|
|
SpaceSW *self = (SpaceSW *)p_self;
|
|
self->collision_pairs--;
|
|
ConstraintSW *c = (ConstraintSW *)p_data;
|
|
memdelete(c);
|
|
}
|
|
|
|
const SelfList<BodySW>::List &SpaceSW::get_active_body_list() const {
|
|
|
|
return active_list;
|
|
}
|
|
void SpaceSW::body_add_to_active_list(SelfList<BodySW> *p_body) {
|
|
|
|
active_list.add(p_body);
|
|
}
|
|
void SpaceSW::body_remove_from_active_list(SelfList<BodySW> *p_body) {
|
|
|
|
active_list.remove(p_body);
|
|
}
|
|
|
|
void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW> *p_body) {
|
|
|
|
inertia_update_list.add(p_body);
|
|
}
|
|
|
|
void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW> *p_body) {
|
|
|
|
inertia_update_list.remove(p_body);
|
|
}
|
|
|
|
BroadPhaseSW *SpaceSW::get_broadphase() {
|
|
|
|
return broadphase;
|
|
}
|
|
|
|
void SpaceSW::add_object(CollisionObjectSW *p_object) {
|
|
|
|
ERR_FAIL_COND(objects.has(p_object));
|
|
objects.insert(p_object);
|
|
}
|
|
|
|
void SpaceSW::remove_object(CollisionObjectSW *p_object) {
|
|
|
|
ERR_FAIL_COND(!objects.has(p_object));
|
|
objects.erase(p_object);
|
|
}
|
|
|
|
const Set<CollisionObjectSW *> &SpaceSW::get_objects() const {
|
|
|
|
return objects;
|
|
}
|
|
|
|
void SpaceSW::body_add_to_state_query_list(SelfList<BodySW> *p_body) {
|
|
|
|
state_query_list.add(p_body);
|
|
}
|
|
void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW> *p_body) {
|
|
|
|
state_query_list.remove(p_body);
|
|
}
|
|
|
|
void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW> *p_area) {
|
|
|
|
monitor_query_list.add(p_area);
|
|
}
|
|
void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area) {
|
|
|
|
monitor_query_list.remove(p_area);
|
|
}
|
|
|
|
void SpaceSW::area_add_to_moved_list(SelfList<AreaSW> *p_area) {
|
|
|
|
area_moved_list.add(p_area);
|
|
}
|
|
|
|
void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW> *p_area) {
|
|
|
|
area_moved_list.remove(p_area);
|
|
}
|
|
|
|
const SelfList<AreaSW>::List &SpaceSW::get_moved_area_list() const {
|
|
|
|
return area_moved_list;
|
|
}
|
|
|
|
void SpaceSW::call_queries() {
|
|
|
|
while (state_query_list.first()) {
|
|
|
|
BodySW *b = state_query_list.first()->self();
|
|
state_query_list.remove(state_query_list.first());
|
|
b->call_queries();
|
|
}
|
|
|
|
while (monitor_query_list.first()) {
|
|
|
|
AreaSW *a = monitor_query_list.first()->self();
|
|
monitor_query_list.remove(monitor_query_list.first());
|
|
a->call_queries();
|
|
}
|
|
}
|
|
|
|
void SpaceSW::setup() {
|
|
|
|
contact_debug_count = 0;
|
|
while (inertia_update_list.first()) {
|
|
inertia_update_list.first()->self()->update_inertias();
|
|
inertia_update_list.remove(inertia_update_list.first());
|
|
}
|
|
}
|
|
|
|
void SpaceSW::update() {
|
|
|
|
broadphase->update();
|
|
}
|
|
|
|
void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
|
|
|
|
switch (p_param) {
|
|
|
|
case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
|
|
case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break;
|
|
}
|
|
}
|
|
|
|
real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
|
|
|
|
switch (p_param) {
|
|
|
|
case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
|
|
case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
|
|
case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
|
|
case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold;
|
|
case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold;
|
|
case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
|
|
case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
|
|
case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
|
|
case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void SpaceSW::lock() {
|
|
|
|
locked = true;
|
|
}
|
|
|
|
void SpaceSW::unlock() {
|
|
|
|
locked = false;
|
|
}
|
|
|
|
bool SpaceSW::is_locked() const {
|
|
|
|
return locked;
|
|
}
|
|
|
|
PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {
|
|
|
|
return direct_access;
|
|
}
|
|
|
|
SpaceSW::SpaceSW() {
|
|
|
|
collision_pairs = 0;
|
|
active_objects = 0;
|
|
island_count = 0;
|
|
contact_debug_count = 0;
|
|
|
|
locked = false;
|
|
contact_recycle_radius = 0.01;
|
|
contact_max_separation = 0.05;
|
|
contact_max_allowed_penetration = 0.01;
|
|
test_motion_min_contact_depth = 0.00001;
|
|
|
|
constraint_bias = 0.01;
|
|
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);
|
|
body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI));
|
|
body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5);
|
|
ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));
|
|
body_angular_velocity_damp_ratio = 10;
|
|
|
|
broadphase = BroadPhaseSW::create_func();
|
|
broadphase->set_pair_callback(_broadphase_pair, this);
|
|
broadphase->set_unpair_callback(_broadphase_unpair, this);
|
|
area = NULL;
|
|
|
|
direct_access = memnew(PhysicsDirectSpaceStateSW);
|
|
direct_access->space = this;
|
|
|
|
for (int i = 0; i < ELAPSED_TIME_MAX; i++)
|
|
elapsed_time[i] = 0;
|
|
}
|
|
|
|
SpaceSW::~SpaceSW() {
|
|
|
|
memdelete(broadphase);
|
|
memdelete(direct_access);
|
|
}
|