/**************************************************************************/
/* jolt_physics_direct_space_state_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* 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 "jolt_physics_direct_space_state_3d.h"
#include "../jolt_physics_server_3d.h"
#include "../jolt_project_settings.h"
#include "../misc/jolt_math_funcs.h"
#include "../misc/jolt_type_conversions.h"
#include "../objects/jolt_area_3d.h"
#include "../objects/jolt_body_3d.h"
#include "../objects/jolt_object_3d.h"
#include "../shapes/jolt_custom_motion_shape.h"
#include "../shapes/jolt_shape_3d.h"
#include "jolt_motion_filter_3d.h"
#include "jolt_query_collectors.h"
#include "jolt_query_filter_3d.h"
#include "jolt_space_3d.h"
#include <Jolt/Geometry/GJKClosestPoint.h>
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/Body/BodyFilter.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
#include <Jolt/Physics/Collision/CastResult.h>
#include <Jolt/Physics/Collision/CollidePointResult.h>
#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
#include <Jolt/Physics/Collision/RayCast.h>
#include <Jolt/Physics/Collision/Shape/MeshShape.h>
#include <Jolt/Physics/PhysicsSystem.h>
bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const {
r_closest_safe = 1.0f;
r_closest_unsafe = 1.0f;
ERR_FAIL_COND_V_MSG(p_jolt_shape.GetType() != JPH::EShapeType::Convex, false, "Shape-casting with non-convex shapes is not supported.");
const float motion_length = (float)p_motion.length();
if (p_ignore_overlaps && motion_length == 0.0f) {
return false;
}
const JPH::RMat44 transform_com = to_jolt_r(p_transform_com);
const JPH::Vec3 scale = to_jolt(p_scale);
const JPH::Vec3 motion = to_jolt(p_motion);
const JPH::Vec3 motion_local = transform_com.Multiply3x3Transposed(motion);
JPH::AABox aabb = p_jolt_shape.GetWorldSpaceBounds(transform_com, scale);
JPH::AABox aabb_translated = aabb;
aabb_translated.Translate(motion);
aabb.Encapsulate(aabb_translated);
JoltQueryCollectorAnyMulti<JPH::CollideShapeBodyCollector, 1024> aabb_collector;
space->get_broad_phase_query().CollideAABox(aabb, aabb_collector, p_broad_phase_layer_filter, p_object_layer_filter);
if (!aabb_collector.had_hit()) {
return false;
}
const JPH::RVec3 base_offset = transform_com.GetTranslation();
JoltCustomMotionShape motion_shape(static_cast<const JPH::ConvexShape &>(p_jolt_shape));
auto collides = [&](const JPH::Body &p_other_body, float p_fraction) {
motion_shape.set_motion(motion_local * p_fraction);
const JPH::TransformedShape other_shape = p_other_body.GetTransformedShape();
JoltQueryCollectorAny<JPH::CollideShapeCollector> collector;
if (p_use_edge_removal) {
JPH::CollideShapeSettings eier_settings = p_settings;
eier_settings.mActiveEdgeMode = JPH::EActiveEdgeMode::CollideWithAll;
eier_settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
JPH::InternalEdgeRemovingCollector eier_collector(collector, JPH::cDefaultInternalEdgeRemovalVertexToleranceSq);
other_shape.CollideShape(&motion_shape, scale, transform_com, eier_settings, base_offset, eier_collector, p_shape_filter);
eier_collector.Flush();
} else {
other_shape.CollideShape(&motion_shape, scale, transform_com, p_settings, base_offset, collector, p_shape_filter);
}
return collector.had_hit();
};
// Figure out the number of steps we need in our binary search in order to achieve millimeter precision, within reason.
const int step_count = CLAMP(int(std::log(1000.0f * motion_length) / (float)Math::LN2), 4, 16);
bool collided = false;
for (int i = 0; i < aabb_collector.get_hit_count(); ++i) {
const JPH::BodyID other_jolt_id = aabb_collector.get_hit(i);
if (!p_body_filter.ShouldCollide(other_jolt_id)) {
continue;
}
const JPH::Body *other_jolt_body = space->try_get_jolt_body(other_jolt_id);
if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) {
continue;
}
if (!collides(*other_jolt_body, 1.0f)) {
continue;
}
if (p_ignore_overlaps && collides(*other_jolt_body, 0.0f)) {
continue;
}
float lo = 0.0f;
float hi = 1.0f;
float coeff = 0.5f;
for (int j = 0; j < step_count; ++j) {
const float fraction = lo + (hi - lo) * coeff;
if (collides(*other_jolt_body, fraction)) {
collided = true;
hi = fraction;
if (j == 0 || lo > 0.0f) {
coeff = 0.5f;
} else {
coeff = 0.25f;
}
} else {
lo = fraction;
if (j == 0 || hi < 1.0f) {
coeff = 0.5f;
} else {
coeff = 0.75f;
}
}
}
if (lo < r_closest_safe) {
r_closest_safe = lo;
r_closest_unsafe = hi;
}
}
return collided;
}
bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, Vector3 &r_recovery) const {
const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
Transform3D transform_com = p_transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mMaxSeparationDistance = p_margin;
const Vector3 &base_offset = transform_com.origin;
const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector;
bool recovered = false;
for (int i = 0; i < JoltProjectSettings::motion_query_recovery_iterations; ++i) {
collector.reset();
_collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
if (!collector.had_hit()) {
break;
}
const int hit_count = collector.get_hit_count();
float combined_priority = 0.0;
for (int j = 0; j < hit_count; j++) {
const JPH::CollideShapeResult &hit = collector.get_hit(j);
const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
ERR_CONTINUE(other_body == nullptr);
combined_priority += other_body->get_collision_priority();
}
const float average_priority = MAX(combined_priority / (float)hit_count, (float)CMP_EPSILON);
recovered = true;
Vector3 recovery;
for (int j = 0; j < hit_count; ++j) {
const JPH::CollideShapeResult &hit = collector.get_hit(j);
const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
const Vector3 margin_offset = penetration_axis * p_margin;
const Vector3 point_on_1 = base_offset + to_godot(hit.mContactPointOn1) + margin_offset;
const Vector3 point_on_2 = base_offset + to_godot(hit.mContactPointOn2);
const real_t distance_to_1 = penetration_axis.dot(point_on_1 + recovery);
const real_t distance_to_2 = penetration_axis.dot(point_on_2);
const float penetration_depth = float(distance_to_1 - distance_to_2);
if (penetration_depth <= 0.0f) {
continue;
}
const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
ERR_CONTINUE(other_body == nullptr);
const float recovery_distance = penetration_depth * JoltProjectSettings::motion_query_recovery_amount;
const float other_priority = other_body->get_collision_priority();
const float other_priority_normalized = other_priority / average_priority;
const float scaled_recovery_distance = recovery_distance * other_priority_normalized;
recovery -= penetration_axis * scaled_recovery_distance;
}
if (recovery == Vector3()) {
break;
}
r_recovery += recovery;
transform_com.origin += recovery;
}
return recovered;
}
bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const {
const Transform3D body_transform = p_transform.scaled_local(p_scale);
const JPH::CollideShapeSettings settings;
const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects, p_collide_separation_ray);
bool collided = false;
for (int i = 0; i < p_body.get_shape_count(); ++i) {
if (p_body.is_shape_disabled(i)) {
continue;
}
JoltShape3D *shape = p_body.get_shape(i);
if (!shape->is_convex()) {
continue;
}
const JPH::ShapeRefC jolt_shape = shape->try_build();
if (unlikely(jolt_shape == nullptr)) {
return false;
}
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
const Transform3D transform_local = p_body.get_shape_transform_scaled(i);
const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
Transform3D transform_com = body_transform * transform_com_local;
Vector3 scale;
JoltMath::decompose(transform_com, scale);
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "body_test_motion was passed an invalid transform along with body '%s'. This results in invalid scaling for shape at index %d.");
real_t shape_safe_fraction = 1.0;
real_t shape_unsafe_fraction = 1.0;
collided |= _cast_motion_impl(*jolt_shape, transform_com, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries, false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction);
r_safe_fraction = MIN(r_safe_fraction, shape_safe_fraction);
r_unsafe_fraction = MIN(r_unsafe_fraction, shape_unsafe_fraction);
}
return collided;
}
bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, PhysicsServer3D::MotionResult *p_result) const {
if (p_max_collisions == 0) {
return false;
}
const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
const Transform3D transform_com = p_transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
settings.mMaxSeparationDistance = p_margin;
const Vector3 &base_offset = transform_com.origin;
const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
JoltQueryCollectorClosestMulti<JPH::CollideShapeCollector, 32> collector(p_max_collisions);
_collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
if (!collector.had_hit() || p_result == nullptr) {
return collector.had_hit();
}
int count = 0;
for (int i = 0; i < collector.get_hit_count(); ++i) {
const JPH::CollideShapeResult &hit = collector.get_hit(i);
const float penetration_depth = hit.mPenetrationDepth + p_margin;
if (penetration_depth <= 0.0f) {
continue;
}
const Vector3 normal = to_godot(-hit.mPenetrationAxis.Normalized());
if (p_motion.length_squared() > 0) {
const Vector3 direction = p_motion.normalized();
if (direction.dot(normal) >= -CMP_EPSILON) {
continue;
}
}
JPH::ContactPoints contact_points1;
JPH::ContactPoints contact_points2;
if (p_max_collisions > 1) {
_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
} else {
contact_points2.push_back(hit.mContactPointOn2);
}
const JoltShapedObject3D *collider = space->try_get_shaped(hit.mBodyID2);
ERR_FAIL_NULL_V(collider, false);
const int local_shape = p_body.find_shape_index(hit.mSubShapeID1);
ERR_FAIL_COND_V(local_shape == -1, false);
const int collider_shape = collider->find_shape_index(hit.mSubShapeID2);
ERR_FAIL_COND_V(collider_shape == -1, false);
for (JPH::Vec3 contact_point : contact_points2) {
const Vector3 position = base_offset + to_godot(contact_point);
PhysicsServer3D::MotionCollision &collision = p_result->collisions[count++];
collision.position = position;
collision.normal = normal;
collision.collider_velocity = collider->get_velocity_at_position(position);
collision.collider_angular_velocity = collider->get_angular_velocity();
collision.depth = penetration_depth;
collision.local_shape = local_shape;
collision.collider_id = collider->get_instance_id();
collision.collider = collider->get_rid();
collision.collider_shape = collider_shape;
if (count == p_max_collisions) {
break;
}
}
if (count == p_max_collisions) {
break;
}
}
p_result->collision_count = count;
return count > 0;
}
int JoltPhysicsDirectSpaceState3D::_try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id) {
if (!JoltProjectSettings::enable_ray_cast_face_index) {
return -1;
}
const JPH::Shape *root_shape = p_body.GetShape();
JPH::SubShapeID sub_shape_id_remainder;
const JPH::Shape *leaf_shape = root_shape->GetLeafShape(p_sub_shape_id, sub_shape_id_remainder);
if (leaf_shape->GetType() != JPH::EShapeType::Mesh) {
return -1;
}
const JPH::MeshShape *mesh_shape = static_cast<const JPH::MeshShape *>(leaf_shape);
return (int)mesh_shape->GetTriangleUserData(sub_shape_id_remainder);
}
void JoltPhysicsDirectSpaceState3D::_generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const {
const JPH::PhysicsSystem &physics_system = space->get_physics_system();
const JPH::PhysicsSettings &physics_settings = physics_system.GetPhysicsSettings();
const JPH::Vec3 penetration_axis = p_hit.mPenetrationAxis.Normalized();
JPH::ManifoldBetweenTwoFaces(p_hit.mContactPointOn1, p_hit.mContactPointOn2, penetration_axis, physics_settings.mManifoldTolerance, p_hit.mShape1Face, p_hit.mShape2Face, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
if (r_contact_points1.size() > 4) {
JPH::PruneContactPoints(penetration_axis, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
}
}
void JoltPhysicsDirectSpaceState3D::_collide_shape_queries(
const JPH::Shape *p_shape,
JPH::Vec3Arg p_scale,
JPH::RMat44Arg p_transform_com,
const JPH::CollideShapeSettings &p_settings,
JPH::RVec3Arg p_base_offset,
JPH::CollideShapeCollector &p_collector,
const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
const JPH::ObjectLayerFilter &p_object_layer_filter,
const JPH::BodyFilter &p_body_filter,
const JPH::ShapeFilter &p_shape_filter) const {
if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries) {
space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
} else {
space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
}
}
void JoltPhysicsDirectSpaceState3D::_collide_shape_kinematics(
const JPH::Shape *p_shape,
JPH::Vec3Arg p_scale,
JPH::RMat44Arg p_transform_com,
const JPH::CollideShapeSettings &p_settings,
JPH::RVec3Arg p_base_offset,
JPH::CollideShapeCollector &p_collector,
const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
const JPH::ObjectLayerFilter &p_object_layer_filter,
const JPH::BodyFilter &p_body_filter,
const JPH::ShapeFilter &p_shape_filter) const {
if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries) {
space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
} else {
space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
}
}
JoltPhysicsDirectSpaceState3D::JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space) :
space(p_space) {
}
bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_ray must not be called while the physics space is being stepped.");
space->flush_pending_objects();
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude, p_parameters.pick_ray);
const JPH::RVec3 from = to_jolt_r(p_parameters.from);
const JPH::RVec3 to = to_jolt_r(p_parameters.to);
const JPH::Vec3 vector = JPH::Vec3(to - from);
const JPH::RRayCast ray(from, vector);
const JPH::EBackFaceMode back_face_mode = p_parameters.hit_back_faces ? JPH::EBackFaceMode::CollideWithBackFaces : JPH::EBackFaceMode::IgnoreBackFaces;
JPH::RayCastSettings settings;
settings.mTreatConvexAsSolid = p_parameters.hit_from_inside;
settings.mBackFaceModeTriangles = back_face_mode;
JoltQueryCollectorClosest<JPH::CastRayCollector> collector;
space->get_narrow_phase_query().CastRay(ray, settings, collector, query_filter, query_filter, query_filter);
if (!collector.had_hit()) {
return false;
}
const JPH::RayCastResult &hit = collector.get_hit();
const JPH::BodyID &body_id = hit.mBodyID;
const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2;
const JoltObject3D *object = space->try_get_object(body_id);
ERR_FAIL_NULL_V(object, false);
const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction);
JPH::Vec3 normal = JPH::Vec3::sZero();
if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) {
normal = object->get_jolt_body()->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
// If we got a back-face normal we need to flip it.
if (normal.Dot(vector) > 0) {
normal = -normal;
}
}
r_result.position = to_godot(position);
r_result.normal = to_godot(normal);
r_result.rid = object->get_rid();
r_result.collider_id = object->get_instance_id();
r_result.collider = object->get_instance();
r_result.shape = 0;
if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
const int shape_index = shaped_object->find_shape_index(sub_shape_id);
ERR_FAIL_COND_V(shape_index == -1, false);
r_result.shape = shape_index;
r_result.face_index = _try_get_face_index(*object->get_jolt_body(), sub_shape_id);
}
return true;
}
int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_point must not be called while the physics space is being stepped.");
if (p_result_max == 0) {
return 0;
}
space->flush_pending_objects();
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
JoltQueryCollectorAnyMulti<JPH::CollidePointCollector, 32> collector(p_result_max);
space->get_narrow_phase_query().CollidePoint(to_jolt_r(p_parameters.position), collector, query_filter, query_filter, query_filter);
const int hit_count = collector.get_hit_count();
for (int i = 0; i < hit_count; ++i) {
const JPH::CollidePointResult &hit = collector.get_hit(i);
const JoltObject3D *object = space->try_get_object(hit.mBodyID);
ERR_FAIL_NULL_V(object, 0);
ShapeResult &result = *r_results++;
result.shape = 0;
if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
ERR_FAIL_COND_V(shape_index == -1, 0);
result.shape = shape_index;
}
result.rid = object->get_rid();
result.collider_id = object->get_instance_id();
result.collider = object->get_instance();
}
return hit_count;
}
int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_shape must not be called while the physics space is being stepped.");
if (p_result_max == 0) {
return 0;
}
space->flush_pending_objects();
JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
ERR_FAIL_NULL_V(shape, 0);
const JPH::ShapeRefC jolt_shape = shape->try_build();
ERR_FAIL_NULL_V(jolt_shape, 0);
Transform3D transform = p_parameters.transform;
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
Vector3 scale;
JoltMath::decompose(transform, scale);
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
const Transform3D transform_com = transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mMaxSeparationDistance = (float)p_parameters.margin;
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(transform_com.origin), collector, query_filter, query_filter, query_filter);
const int hit_count = collector.get_hit_count();
for (int i = 0; i < hit_count; ++i) {
const JPH::CollideShapeResult &hit = collector.get_hit(i);
const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
ERR_FAIL_NULL_V(object, 0);
ShapeResult &result = *r_results++;
result.shape = 0;
if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
ERR_FAIL_COND_V(shape_index == -1, 0);
result.shape = shape_index;
}
result.rid = object->get_rid();
result.collider_id = object->get_instance_id();
result.collider = object->get_instance();
}
return hit_count;
}
bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info) {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "cast_motion must not be called while the physics space is being stepped.");
ERR_FAIL_COND_V_MSG(r_info != nullptr, false, "Providing rest info as part of cast_motion is not supported when using Jolt Physics.");
space->flush_pending_objects();
JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
ERR_FAIL_NULL_V(shape, false);
const JPH::ShapeRefC jolt_shape = shape->try_build();
ERR_FAIL_NULL_V(jolt_shape, false);
Transform3D transform = p_parameters.transform;
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
Vector3 scale;
JoltMath::decompose(transform, scale);
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
Transform3D transform_com = transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mMaxSeparationDistance = (float)p_parameters.margin;
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
_cast_motion_impl(*jolt_shape, transform_com, scale, p_parameters.motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries, true, settings, query_filter, query_filter, query_filter, JPH::ShapeFilter(), r_closest_safe, r_closest_unsafe);
return true;
}
bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) {
r_result_count = 0;
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "collide_shape must not be called while the physics space is being stepped.");
if (p_result_max == 0) {
return false;
}
space->flush_pending_objects();
JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
ERR_FAIL_NULL_V(shape, false);
const JPH::ShapeRefC jolt_shape = shape->try_build();
ERR_FAIL_NULL_V(jolt_shape, false);
Transform3D transform = p_parameters.transform;
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
Vector3 scale;
JoltMath::decompose(transform, scale);
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
const Transform3D transform_com = transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
settings.mMaxSeparationDistance = (float)p_parameters.margin;
const Vector3 &base_offset = transform_com.origin;
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
if (!collector.had_hit()) {
return false;
}
const int max_points = p_result_max * 2;
int point_count = 0;
for (int i = 0; i < collector.get_hit_count(); ++i) {
const JPH::CollideShapeResult &hit = collector.get_hit(i);
const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
const Vector3 margin_offset = penetration_axis * (float)p_parameters.margin;
JPH::ContactPoints contact_points1;
JPH::ContactPoints contact_points2;
_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
for (JPH::uint j = 0; j < contact_points1.size(); ++j) {
r_results[point_count++] = base_offset + to_godot(contact_points1[j]) + margin_offset;
r_results[point_count++] = base_offset + to_godot(contact_points2[j]);
if (point_count >= max_points) {
break;
}
}
if (point_count >= max_points) {
break;
}
}
r_result_count = point_count / 2;
return true;
}
bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "get_rest_info must not be called while the physics space is being stepped.");
space->flush_pending_objects();
JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
ERR_FAIL_NULL_V(shape, false);
const JPH::ShapeRefC jolt_shape = shape->try_build();
ERR_FAIL_NULL_V(jolt_shape, false);
Transform3D transform = p_parameters.transform;
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
Vector3 scale;
JoltMath::decompose(transform, scale);
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
const Transform3D transform_com = transform.translated_local(com_scaled);
JPH::CollideShapeSettings settings;
settings.mMaxSeparationDistance = (float)p_parameters.margin;
const Vector3 &base_offset = transform_com.origin;
const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
JoltQueryCollectorClosest<JPH::CollideShapeCollector> collector;
_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
if (!collector.had_hit()) {
return false;
}
const JPH::CollideShapeResult &hit = collector.get_hit();
const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
ERR_FAIL_NULL_V(object, false);
r_info->shape = 0;
if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
ERR_FAIL_COND_V(shape_index == -1, false);
r_info->shape = shape_index;
}
const Vector3 hit_point = base_offset + to_godot(hit.mContactPointOn2);
r_info->point = hit_point;
r_info->normal = to_godot(-hit.mPenetrationAxis.Normalized());
r_info->rid = object->get_rid();
r_info->collider_id = object->get_instance_id();
r_info->linear_velocity = object->get_velocity_at_position(hit_point);
return true;
}
Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const {
ERR_FAIL_COND_V_MSG(space->is_stepping(), Vector3(), "get_closest_point_to_object_volume must not be called while the physics space is being stepped.");
space->flush_pending_objects();
JoltPhysicsServer3D *physics_server = JoltPhysicsServer3D::get_singleton();
JoltObject3D *object = physics_server->get_area(p_object);
if (object == nullptr) {
object = physics_server->get_body(p_object);
}
ERR_FAIL_NULL_V(object, Vector3());
ERR_FAIL_COND_V(object->get_space() != space, Vector3());
JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
const JPH::TransformedShape root_shape = object->get_jolt_body()->GetTransformedShape();
root_shape.CollectTransformedShapes(object->get_jolt_body()->GetWorldSpaceBounds(), collector);
const JPH::RVec3 point = to_jolt_r(p_point);
float closest_distance_sq = FLT_MAX;
JPH::RVec3 closest_point = JPH::RVec3::sZero();
bool found_point = false;
for (int i = 0; i < collector.get_hit_count(); ++i) {
const JPH::TransformedShape &shape_transformed = collector.get_hit(i);
const JPH::Shape &shape = *shape_transformed.mShape;
if (shape.GetType() != JPH::EShapeType::Convex) {
continue;
}
const JPH::ConvexShape &shape_convex = static_cast<const JPH::ConvexShape &>(shape);
JPH::GJKClosestPoint gjk;
JPH::ConvexShape::SupportBuffer shape_support_buffer;
const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale());
const JPH::RMat44 shape_rotation = JPH::RMat44::sRotation(shape_transformed.mShapeRotation);
const JPH::Vec3 shape_com = shape_rotation.Multiply3x3(shape.GetCenterOfMass());
const JPH::RVec3 shape_pos = shape_transformed.mShapePositionCOM - JPH::RVec3(shape_com);
const JPH::RMat44 shape_xform = shape_rotation.PostTranslated(shape_pos);
const JPH::RMat44 shape_xform_inv = shape_xform.InversedRotationTranslation();
JPH::PointConvexSupport point_support;
point_support.mPoint = JPH::Vec3(shape_xform_inv * point);
JPH::Vec3 separating_axis = JPH::Vec3::sAxisX();
JPH::Vec3 point_on_a = JPH::Vec3::sZero();
JPH::Vec3 point_on_b = JPH::Vec3::sZero();
const float distance_sq = gjk.GetClosestPoints(*shape_support, point_support, JPH::cDefaultCollisionTolerance, FLT_MAX, separating_axis, point_on_a, point_on_b);
if (distance_sq == 0.0f) {
closest_point = point;
found_point = true;
break;
}
if (distance_sq < closest_distance_sq) {
closest_distance_sq = distance_sq;
closest_point = shape_xform * point_on_a;
found_point = true;
}
}
if (found_point) {
return to_godot(closest_point);
} else {
return to_godot(object->get_jolt_body()->GetPosition());
}
}
bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const {
ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "body_test_motion (maybe from move_and_slide?) must not be called while the physics space is being stepped.");
if (!p_body.in_space()) {
return false;
}
space->flush_pending_objects();
const float margin = MAX((float)p_parameters.margin, 0.0001f);
const int max_collisions = MIN(p_parameters.max_collisions, 32);
Transform3D transform = p_parameters.from;
JOLT_ENSURE_SCALE_NOT_ZERO(transform, vformat("body_test_motion (maybe from move_and_slide?) was passed an invalid transform along with body '%s'.", p_body.to_string()));
Vector3 scale;
JoltMath::decompose(transform, scale);
Vector3 recovery;
const bool recovered = _body_motion_recover(p_body, transform, margin, p_parameters.exclude_bodies, p_parameters.exclude_objects, recovery);
transform.origin += recovery;
real_t safe_fraction = 1.0;
real_t unsafe_fraction = 1.0;
const bool hit = _body_motion_cast(p_body, transform, scale, p_parameters.motion, p_parameters.collide_separation_ray, p_parameters.exclude_bodies, p_parameters.exclude_objects, safe_fraction, unsafe_fraction);
bool collided = false;
if (hit || (recovered && p_parameters.recovery_as_collision)) {
collided = _body_motion_collide(p_body, transform.translated(p_parameters.motion * unsafe_fraction), p_parameters.motion, margin, max_collisions, p_parameters.exclude_bodies, p_parameters.exclude_objects, r_result);
}
if (r_result == nullptr) {
return collided;
}
if (collided) {
const PhysicsServer3D::MotionCollision &deepest = r_result->collisions[0];
r_result->travel = recovery + p_parameters.motion * safe_fraction;
r_result->remainder = p_parameters.motion - p_parameters.motion * safe_fraction;
r_result->collision_depth = deepest.depth;
r_result->collision_safe_fraction = safe_fraction;
r_result->collision_unsafe_fraction = unsafe_fraction;
} else {
r_result->travel = recovery + p_parameters.motion;
r_result->remainder = Vector3();
r_result->collision_depth = 0.0f;
r_result->collision_safe_fraction = 1.0f;
r_result->collision_unsafe_fraction = 1.0f;
r_result->collision_count = 0;
}
return collided;
}