Sara
c3f9669b10
git-subtree-dir: engine git-subtree-mainline:b74841629egit-subtree-split:a8e37fc010
856 lines
32 KiB
C++
856 lines
32 KiB
C++
/**************************************************************************/
|
|
/* jolt_soft_body_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_soft_body_3d.h"
|
|
|
|
#include "../jolt_project_settings.h"
|
|
#include "../misc/jolt_type_conversions.h"
|
|
#include "../spaces/jolt_broad_phase_layer.h"
|
|
#include "../spaces/jolt_space_3d.h"
|
|
#include "jolt_area_3d.h"
|
|
#include "jolt_body_3d.h"
|
|
#include "jolt_group_filter.h"
|
|
|
|
#include "core/config/engine.h"
|
|
#include "servers/rendering/rendering_server.h"
|
|
|
|
#include "Jolt/Physics/SoftBody/SoftBodyMotionProperties.h"
|
|
|
|
namespace {
|
|
|
|
template <typename TJoltVertex>
|
|
void pin_vertices(const JoltSoftBody3D &p_body, const HashSet<int> &p_pinned_vertices, const LocalVector<int> &p_mesh_to_physics, JPH::Array<TJoltVertex> &r_physics_vertices) {
|
|
const int mesh_vertex_count = p_mesh_to_physics.size();
|
|
const int physics_vertex_count = (int)r_physics_vertices.size();
|
|
|
|
for (int mesh_index : p_pinned_vertices) {
|
|
ERR_CONTINUE_MSG(mesh_index < 0 || mesh_index >= mesh_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh.", mesh_index, p_body.to_string(), mesh_vertex_count));
|
|
|
|
const int physics_index = p_mesh_to_physics[mesh_index];
|
|
ERR_CONTINUE_MSG(physics_index < 0 || physics_index >= physics_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh. This should not happen. Please report this.", physics_index, p_body.to_string(), physics_vertex_count));
|
|
|
|
r_physics_vertices[physics_index].mInvMass = 0.0f;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
JPH::BroadPhaseLayer JoltSoftBody3D::_get_broad_phase_layer() const {
|
|
return JoltBroadPhaseLayer::BODY_DYNAMIC;
|
|
}
|
|
|
|
JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const {
|
|
ERR_FAIL_NULL_V(space, 0);
|
|
|
|
return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
|
|
}
|
|
|
|
void JoltSoftBody3D::_space_changing() {
|
|
JoltObject3D::_space_changing();
|
|
|
|
if (in_space()) {
|
|
jolt_settings = new JPH::SoftBodyCreationSettings(jolt_body->GetSoftBodyCreationSettings());
|
|
jolt_settings->mSettings = nullptr;
|
|
jolt_settings->mVertexRadius = JoltProjectSettings::soft_body_point_radius;
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::_space_changed() {
|
|
JoltObject3D::_space_changed();
|
|
|
|
_update_mass();
|
|
_update_pressure();
|
|
_update_damping();
|
|
_update_simulation_precision();
|
|
_update_group_filter();
|
|
}
|
|
|
|
void JoltSoftBody3D::_add_to_space() {
|
|
if (unlikely(space == nullptr || !mesh.is_valid())) {
|
|
return;
|
|
}
|
|
|
|
JPH::SoftBodySharedSettings *shared_settings = _create_shared_settings();
|
|
ERR_FAIL_NULL(shared_settings);
|
|
|
|
JPH::CollisionGroup::GroupID group_id = 0;
|
|
JPH::CollisionGroup::SubGroupID sub_group_id = 0;
|
|
JoltGroupFilter::encode_object(this, group_id, sub_group_id);
|
|
|
|
jolt_settings->mSettings = shared_settings;
|
|
jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
|
|
jolt_settings->mObjectLayer = _get_object_layer();
|
|
jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
|
|
jolt_settings->mMaxLinearVelocity = JoltProjectSettings::max_linear_velocity;
|
|
|
|
JPH::Body *new_jolt_body = space->add_object(*this, *jolt_settings);
|
|
if (new_jolt_body == nullptr) {
|
|
return;
|
|
}
|
|
|
|
jolt_body = new_jolt_body;
|
|
|
|
delete jolt_settings;
|
|
jolt_settings = nullptr;
|
|
}
|
|
|
|
JPH::SoftBodySharedSettings *JoltSoftBody3D::_create_shared_settings() {
|
|
RenderingServer *rendering = RenderingServer::get_singleton();
|
|
|
|
// TODO: calling RenderingServer::mesh_surface_get_arrays() from the physics thread
|
|
// is not safe and can deadlock when physics/3d/run_on_separate_thread is enabled.
|
|
// This method blocks on the main thread to return data, but the main thread may be
|
|
// blocked waiting on us in PhysicsServer3D::sync().
|
|
const Array mesh_data = rendering->mesh_surface_get_arrays(mesh, 0);
|
|
ERR_FAIL_COND_V(mesh_data.is_empty(), nullptr);
|
|
|
|
const PackedInt32Array mesh_indices = mesh_data[RSE::ARRAY_INDEX];
|
|
ERR_FAIL_COND_V(mesh_indices.is_empty(), nullptr);
|
|
|
|
const PackedVector3Array mesh_vertices = mesh_data[RSE::ARRAY_VERTEX];
|
|
ERR_FAIL_COND_V(mesh_vertices.is_empty(), nullptr);
|
|
|
|
JPH::SoftBodySharedSettings *settings = new JPH::SoftBodySharedSettings();
|
|
JPH::Array<JPH::SoftBodySharedSettings::Vertex> &physics_vertices = settings->mVertices;
|
|
JPH::Array<JPH::SoftBodySharedSettings::Face> &physics_faces = settings->mFaces;
|
|
|
|
HashMap<Vector3, int> vertex_to_physics;
|
|
|
|
const int mesh_vertex_count = mesh_vertices.size();
|
|
const int mesh_index_count = mesh_indices.size();
|
|
|
|
mesh_to_physics.resize(mesh_vertex_count);
|
|
for (int &index : mesh_to_physics) {
|
|
index = -1;
|
|
}
|
|
physics_vertices.reserve(mesh_vertex_count);
|
|
vertex_to_physics.reserve(mesh_vertex_count);
|
|
|
|
int physics_index_count = 0;
|
|
|
|
const JPH::RVec3 body_position = jolt_settings->mPosition;
|
|
|
|
for (int i = 0; i < mesh_index_count; i += 3) {
|
|
int physics_face[3];
|
|
|
|
for (int j = 0; j < 3; ++j) {
|
|
const int mesh_index = mesh_indices[i + j];
|
|
const Vector3 vertex = mesh_vertices[mesh_index];
|
|
|
|
HashMap<Vector3, int>::Iterator iter_physics_index = vertex_to_physics.find(vertex);
|
|
|
|
if (iter_physics_index == vertex_to_physics.end()) {
|
|
physics_vertices.emplace_back(JPH::Float3((float)(vertex.x - body_position.GetX()), (float)(vertex.y - body_position.GetY()), (float)(vertex.z - body_position.GetZ())), JPH::Float3(0.0f, 0.0f, 0.0f), 1.0f);
|
|
iter_physics_index = vertex_to_physics.insert(vertex, physics_index_count++);
|
|
}
|
|
|
|
physics_face[j] = iter_physics_index->value;
|
|
mesh_to_physics[mesh_index] = iter_physics_index->value;
|
|
}
|
|
|
|
if (physics_face[0] == physics_face[1] || physics_face[0] == physics_face[2] || physics_face[1] == physics_face[2]) {
|
|
continue; // We skip degenerate faces, since they're problematic, and Jolt will assert about it anyway.
|
|
}
|
|
|
|
// Jolt uses a different winding order, so we swap the indices to account for that.
|
|
physics_faces.emplace_back((JPH::uint32)physics_face[2], (JPH::uint32)physics_face[1], (JPH::uint32)physics_face[0]);
|
|
}
|
|
|
|
// Pin whatever pinned vertices we have currently. This is used during the `Optimize` call below to order the
|
|
// constraints. Note that it's fine if the pinned vertices change later, but that will reduce the effectiveness
|
|
// of the constraints a bit.
|
|
pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices);
|
|
|
|
// Since Godot's stiffness is input as a coefficient between 0 and 1, and Jolt uses actual stiffness for its
|
|
// edge constraints, we must map one to the other.
|
|
//
|
|
// Godot uses classic PBD edge constraints, which have a stiffness parameter k that is used in the position correction formula as follows:
|
|
// delta_x1 = -k * w1 / (w1 + w2) * (l - l0) / l * (x2 - x1)
|
|
// where k is the stiffness, w1 and w2 are the inverse masses of the two vertices, l is the current length of the edge = |x2 - x1|, l0 is the rest length of the edge, and x1 and x2 are the vertex positions.
|
|
//
|
|
// Note that the actual formula used in Godot physics seems to use an approximation of this which avoids calculating the square root:
|
|
// delta_x1 = -k * w1 / (w1 + w2) * (l^2 - l0^2) / (l^2 + l0^2) * (x2 - x1)
|
|
//
|
|
// Jolt uses XPBD which goes as follows:
|
|
// delta_x1 = -w1 / (w1 + w2 + compliance / dt^2) * (l - l0) / l * (x2 - x1)
|
|
// where compliance is the inverse of stiffness and dt is the timestep.
|
|
//
|
|
// We can derive Jolt's compliance from Godot's stiffness by evaluating:
|
|
// k * w1 / (w1 + w2) = w1 / (w1 + w2 + compliance / dt^2)
|
|
// which simplifies to:
|
|
// compliance = dt^2 * (1 / k - 1) * (w1 + w2)
|
|
|
|
// Assuming that the vertices have the same mass:
|
|
const float w1_plus_w2 = 2.0f * physics_vertices.size() / mass;
|
|
|
|
// Calculate time step of a single XPBD iteration
|
|
const float dt = 1.0f / Engine::get_singleton()->get_user_physics_ticks_per_second() / simulation_precision;
|
|
|
|
// Now calculate the compliance
|
|
const float inverse_stiffness = dt * dt * (1.0f / stiffness_coefficient - 1.0f) * w1_plus_w2;
|
|
|
|
JPH::SoftBodySharedSettings::VertexAttributes vertex_attrib;
|
|
vertex_attrib.mCompliance = vertex_attrib.mShearCompliance = inverse_stiffness;
|
|
|
|
settings->CreateConstraints(&vertex_attrib, 1, JPH::SoftBodySharedSettings::EBendType::None);
|
|
float multiplier = 1.0f - shrinking_factor;
|
|
for (JPH::SoftBodySharedSettings::Edge &e : settings->mEdgeConstraints) {
|
|
e.mRestLength *= multiplier;
|
|
}
|
|
settings->Optimize();
|
|
|
|
return settings;
|
|
}
|
|
|
|
void JoltSoftBody3D::_apply_environmental_forces(float p_step, JPH::Body &p_jolt_body) {
|
|
// Get approximation of the center of the soft body.
|
|
Vector3 com_position = to_godot(p_jolt_body.GetCenterOfMassPosition());
|
|
|
|
// Calculate gravity and which areas affect the soft body through wind.
|
|
bool gravity_done = false;
|
|
Vector3 gravity;
|
|
LocalVector<JoltArea3D *> wind_areas;
|
|
for (JoltArea3D *area : areas) {
|
|
if (!gravity_done) {
|
|
gravity_done = JoltArea3D::apply_override(gravity, area->get_gravity_mode(), [&]() {
|
|
return area->compute_gravity(com_position);
|
|
});
|
|
}
|
|
|
|
if (area->get_wind_pressure() > CMP_EPSILON) {
|
|
wind_areas.push_back(area);
|
|
}
|
|
}
|
|
|
|
// Add default gravity.
|
|
if (!gravity_done) {
|
|
gravity += space->get_default_area()->compute_gravity(com_position);
|
|
}
|
|
|
|
// Apply gravity to soft body. Note that this only works so long as vertices have uniform mass (excluding pinned vertices).
|
|
p_jolt_body.AddForce(to_jolt(gravity) * mass);
|
|
|
|
if (!wind_areas.is_empty()) {
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*p_jolt_body.GetMotionPropertiesUnchecked());
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
|
|
for (const JPH::SoftBodySharedSettings::Face &physics_face : motion_properties.GetFaces()) {
|
|
JPH::SoftBodyVertex &physics_vertex0 = physics_vertices[physics_face.mVertex[0]];
|
|
JPH::SoftBodyVertex &physics_vertex1 = physics_vertices[physics_face.mVertex[1]];
|
|
JPH::SoftBodyVertex &physics_vertex2 = physics_vertices[physics_face.mVertex[2]];
|
|
|
|
// Calculate the triangle centroid.
|
|
Vector3 v0 = to_godot(physics_vertex0.mPosition);
|
|
Vector3 v1 = to_godot(physics_vertex1.mPosition);
|
|
Vector3 v2 = to_godot(physics_vertex2.mPosition);
|
|
Vector3 centroid = com_position + (v0 + v1 + v2) * real_t(1.0 / 3.0);
|
|
|
|
// Calculate the triangle normal.
|
|
Vector3 normal = (v2 - v0).cross(v1 - v0);
|
|
real_t normal_length = normal.length();
|
|
if (normal_length > real_t(1.0e-6)) { // If the normal is near zero, the area is near zero so we can skip this triangle.
|
|
normal /= normal_length;
|
|
|
|
// Area is half the length of the cross product of two sides.
|
|
real_t triangle_area = real_t(0.5) * normal_length;
|
|
|
|
// Accumulate wind forces from all wind areas.
|
|
Vector3 wind_force;
|
|
for (const JoltArea3D *area : wind_areas) {
|
|
const Vector3 &wind_direction = area->get_wind_direction();
|
|
const Vector3 &wind_source = area->get_wind_source();
|
|
|
|
// Calculate attenuation factor based on distance from wind source to triangle centroid.
|
|
// We do not allow a projection below 1 to ensure that we never amplify and to avoid NaNs when the value would be negative.
|
|
real_t projection_toward_centroid = MAX((centroid - wind_source).dot(wind_direction), real_t(1.0));
|
|
real_t attenuation_over_distance = Math::pow(projection_toward_centroid, -real_t(area->get_wind_attenuation_factor()));
|
|
|
|
// Calculate force magnitude.
|
|
real_t force_magnitude = area->get_wind_pressure() * triangle_area;
|
|
|
|
// Calculate the resulting wind force on the triangle by projecting wind direction onto triangle normal.
|
|
// Divide by 3 to distribute force equally over each vertex.
|
|
wind_force += (force_magnitude * attenuation_over_distance * real_t(1.0 / 3.0) * normal.dot(wind_direction)) * normal;
|
|
}
|
|
|
|
// Apply the force as an impulse over the timestep.
|
|
JPH::Vec3 impulse = to_jolt(wind_force * p_step);
|
|
physics_vertex0.mVelocity += impulse * physics_vertex0.mInvMass;
|
|
physics_vertex1.mVelocity += impulse * physics_vertex1.mInvMass;
|
|
physics_vertex2.mVelocity += impulse * physics_vertex2.mInvMass;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::_update_mass() {
|
|
if (!in_space()) {
|
|
return;
|
|
}
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
|
|
const float inverse_vertex_mass = (float)physics_vertices.size() / mass;
|
|
|
|
for (JPH::SoftBodyVertex &vertex : physics_vertices) {
|
|
vertex.mInvMass = inverse_vertex_mass;
|
|
}
|
|
|
|
pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices);
|
|
}
|
|
|
|
void JoltSoftBody3D::_update_pressure() {
|
|
if (!in_space()) {
|
|
jolt_settings->mPressure = pressure;
|
|
return;
|
|
}
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
motion_properties.SetPressure(pressure);
|
|
}
|
|
|
|
void JoltSoftBody3D::_update_damping() {
|
|
if (!in_space()) {
|
|
jolt_settings->mLinearDamping = linear_damping;
|
|
return;
|
|
}
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
motion_properties.SetLinearDamping(linear_damping);
|
|
}
|
|
|
|
void JoltSoftBody3D::_update_simulation_precision() {
|
|
if (!in_space()) {
|
|
jolt_settings->mNumIterations = (JPH::uint32)simulation_precision;
|
|
return;
|
|
}
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
motion_properties.SetNumIterations((JPH::uint32)simulation_precision);
|
|
}
|
|
|
|
void JoltSoftBody3D::_update_group_filter() {
|
|
JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr;
|
|
|
|
if (!in_space()) {
|
|
jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
|
|
} else {
|
|
jolt_body->GetCollisionGroup().SetGroupFilter(group_filter);
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::_try_rebuild() {
|
|
if (space != nullptr) {
|
|
_reset_space();
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::_mesh_changed() {
|
|
_try_rebuild();
|
|
}
|
|
|
|
void JoltSoftBody3D::_simulation_precision_changed() {
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_mass_changed() {
|
|
_update_mass();
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_pressure_changed() {
|
|
_update_pressure();
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_damping_changed() {
|
|
_update_damping();
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_pins_changed() {
|
|
_update_mass();
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_vertices_changed() {
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_exceptions_changed() {
|
|
_update_group_filter();
|
|
}
|
|
|
|
void JoltSoftBody3D::_motion_changed() {
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_transform_changed() {
|
|
wake_up();
|
|
}
|
|
|
|
void JoltSoftBody3D::_areas_changed() {
|
|
wake_up();
|
|
}
|
|
|
|
JoltSoftBody3D::JoltSoftBody3D() :
|
|
JoltObject3D(OBJECT_TYPE_SOFT_BODY) {
|
|
jolt_settings->mRestitution = 0.0f;
|
|
jolt_settings->mFriction = 1.0f;
|
|
jolt_settings->mUpdatePosition = true;
|
|
jolt_settings->mMakeRotationIdentity = false;
|
|
}
|
|
|
|
JoltSoftBody3D::~JoltSoftBody3D() {
|
|
if (jolt_settings != nullptr) {
|
|
delete jolt_settings;
|
|
jolt_settings = nullptr;
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::add_collision_exception(const RID &p_excepted_body) {
|
|
exceptions.push_back(p_excepted_body);
|
|
|
|
_exceptions_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::remove_collision_exception(const RID &p_excepted_body) {
|
|
exceptions.erase(p_excepted_body);
|
|
|
|
_exceptions_changed();
|
|
}
|
|
|
|
bool JoltSoftBody3D::has_collision_exception(const RID &p_excepted_body) const {
|
|
return exceptions.find(p_excepted_body) >= 0;
|
|
}
|
|
|
|
void JoltSoftBody3D::add_area(JoltArea3D *p_area) {
|
|
int i = 0;
|
|
for (; i < (int)areas.size(); i++) {
|
|
if (p_area->get_priority() > areas[i]->get_priority()) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
areas.insert(i, p_area);
|
|
|
|
_areas_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::remove_area(JoltArea3D *p_area) {
|
|
areas.erase(p_area);
|
|
|
|
_areas_changed();
|
|
}
|
|
|
|
bool JoltSoftBody3D::can_interact_with(const JoltBody3D &p_other) const {
|
|
return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
|
|
}
|
|
|
|
bool JoltSoftBody3D::can_interact_with(const JoltSoftBody3D &p_other) const {
|
|
return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
|
|
}
|
|
|
|
bool JoltSoftBody3D::can_interact_with(const JoltArea3D &p_other) const {
|
|
return p_other.can_interact_with(*this);
|
|
}
|
|
|
|
Vector3 JoltSoftBody3D::get_velocity_at_position(const Vector3 &p_position) const {
|
|
return Vector3();
|
|
}
|
|
|
|
void JoltSoftBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
|
|
_apply_environmental_forces(p_step, p_jolt_body);
|
|
}
|
|
|
|
void JoltSoftBody3D::set_mesh(const RID &p_mesh) {
|
|
if (unlikely(mesh == p_mesh)) {
|
|
return;
|
|
}
|
|
|
|
mesh = p_mesh;
|
|
_mesh_changed();
|
|
}
|
|
|
|
bool JoltSoftBody3D::is_sleeping() const {
|
|
if (!in_space()) {
|
|
return false;
|
|
} else {
|
|
return !jolt_body->IsActive();
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::apply_vertex_impulse(int p_index, const Vector3 &p_impulse) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
ERR_FAIL_INDEX(p_index, (int)mesh_to_physics.size());
|
|
const int physics_index = mesh_to_physics[p_index];
|
|
ERR_FAIL_COND_MSG(physics_index < 0, vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. No impulse can be applied.", p_index, to_string()));
|
|
ERR_FAIL_COND_MSG(pinned_vertices.has(physics_index), vformat("Failed to apply impulse to point at index %d for '%s'. Point was found to be pinned.", static_cast<int>(physics_index), to_string()));
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
|
|
|
|
physics_vertex.mVelocity += to_jolt(p_impulse) * physics_vertex.mInvMass;
|
|
|
|
_motion_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::apply_vertex_force(int p_index, const Vector3 &p_force) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
apply_vertex_impulse(p_index, p_force * space->get_last_step());
|
|
}
|
|
|
|
void JoltSoftBody3D::apply_central_impulse(const Vector3 &p_impulse) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
|
|
const JPH::Vec3 impulse = to_jolt(p_impulse) / physics_vertices.size();
|
|
|
|
for (JPH::SoftBodyVertex &physics_vertex : physics_vertices) {
|
|
if (physics_vertex.mInvMass > 0.0f) {
|
|
physics_vertex.mVelocity += impulse * physics_vertex.mInvMass;
|
|
}
|
|
}
|
|
|
|
_motion_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::apply_central_force(const Vector3 &p_force) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
jolt_body->AddForce(to_jolt(p_force));
|
|
|
|
_motion_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
|
|
if (!in_space()) {
|
|
return;
|
|
}
|
|
|
|
space->set_is_object_sleeping(jolt_body->GetID(), p_enabled);
|
|
}
|
|
|
|
bool JoltSoftBody3D::is_sleep_allowed() const {
|
|
if (!in_space()) {
|
|
return jolt_settings->mAllowSleeping;
|
|
} else {
|
|
return jolt_body->GetAllowSleeping();
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::set_is_sleep_allowed(bool p_enabled) {
|
|
if (!in_space()) {
|
|
jolt_settings->mAllowSleeping = p_enabled;
|
|
} else {
|
|
jolt_body->SetAllowSleeping(p_enabled);
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::set_simulation_precision(int p_precision) {
|
|
if (unlikely(simulation_precision == p_precision)) {
|
|
return;
|
|
}
|
|
|
|
simulation_precision = MAX(p_precision, 0);
|
|
|
|
_simulation_precision_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::set_mass(float p_mass) {
|
|
ERR_FAIL_COND(p_mass <= 0.0); // A mass of zero would result in infinite inverse mass.
|
|
|
|
if (unlikely(mass == p_mass)) {
|
|
return;
|
|
}
|
|
|
|
mass = p_mass;
|
|
|
|
_mass_changed();
|
|
}
|
|
|
|
float JoltSoftBody3D::get_stiffness_coefficient() const {
|
|
return stiffness_coefficient;
|
|
}
|
|
|
|
void JoltSoftBody3D::set_stiffness_coefficient(float p_coefficient) {
|
|
stiffness_coefficient = CLAMP(p_coefficient, 0.0f, 1.0f);
|
|
}
|
|
|
|
float JoltSoftBody3D::get_shrinking_factor() const {
|
|
return shrinking_factor;
|
|
}
|
|
|
|
void JoltSoftBody3D::set_shrinking_factor(float p_shrinking_factor) {
|
|
shrinking_factor = p_shrinking_factor;
|
|
}
|
|
|
|
void JoltSoftBody3D::set_pressure(float p_pressure) {
|
|
if (unlikely(pressure == p_pressure)) {
|
|
return;
|
|
}
|
|
|
|
pressure = MAX(p_pressure, 0.0f);
|
|
|
|
_pressure_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::set_linear_damping(float p_damping) {
|
|
if (unlikely(linear_damping == p_damping)) {
|
|
return;
|
|
}
|
|
|
|
linear_damping = MAX(p_damping, 0.0f);
|
|
|
|
_damping_changed();
|
|
}
|
|
|
|
float JoltSoftBody3D::get_drag() const {
|
|
// Drag is not a thing in Jolt, and not supported by Godot Physics either.
|
|
return 0.0f;
|
|
}
|
|
|
|
void JoltSoftBody3D::set_drag(float p_drag) {
|
|
// Drag is not a thing in Jolt, and not supported by Godot Physics either.
|
|
}
|
|
|
|
Variant JoltSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
|
|
switch (p_state) {
|
|
case PhysicsServer3D::BODY_STATE_TRANSFORM: {
|
|
return get_transform();
|
|
}
|
|
case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
|
|
ERR_FAIL_V_MSG(Variant(), "Linear velocity is not supported for soft bodies.");
|
|
}
|
|
case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
|
|
ERR_FAIL_V_MSG(Variant(), "Angular velocity is not supported for soft bodies.");
|
|
}
|
|
case PhysicsServer3D::BODY_STATE_SLEEPING: {
|
|
return is_sleeping();
|
|
}
|
|
case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
|
|
return is_sleep_allowed();
|
|
}
|
|
default: {
|
|
ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
|
|
}
|
|
}
|
|
}
|
|
|
|
void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) {
|
|
switch (p_state) {
|
|
case PhysicsServer3D::BODY_STATE_TRANSFORM: {
|
|
set_transform(p_value);
|
|
} break;
|
|
case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
|
|
ERR_FAIL_MSG("Linear velocity is not supported for soft bodies.");
|
|
} break;
|
|
case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
|
|
ERR_FAIL_MSG("Angular velocity is not supported for soft bodies.");
|
|
} break;
|
|
case PhysicsServer3D::BODY_STATE_SLEEPING: {
|
|
set_is_sleeping(p_value);
|
|
} break;
|
|
case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
|
|
set_is_sleep_allowed(p_value);
|
|
} break;
|
|
default: {
|
|
ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
|
|
} break;
|
|
}
|
|
}
|
|
|
|
Transform3D JoltSoftBody3D::get_transform() const {
|
|
// Since any transform gets baked into the vertices anyway we can just return identity here.
|
|
return Transform3D();
|
|
}
|
|
|
|
void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set transform for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
// For whatever reason this has to be interpreted as a relative global-space transform rather than an absolute one,
|
|
// because `SoftBody3D` will immediately upon entering the scene tree set itself to be top-level and also set its
|
|
// transform to be identity, while still expecting to stay in its original position.
|
|
//
|
|
// We also discard any scaling, since we have no way of scaling the actual edge lengths.
|
|
const JPH::Mat44 relative_transform = to_jolt(p_transform.orthonormalized());
|
|
|
|
// The translation delta goes to the body's position to avoid vertices getting too far away from it.
|
|
JPH::BodyInterface &body_iface = space->get_body_iface();
|
|
body_iface.SetPosition(jolt_body->GetID(), jolt_body->GetPosition() + relative_transform.GetTranslation(), JPH::EActivation::DontActivate);
|
|
|
|
// The rotation difference goes to the vertices. We also reset the velocity of these vertices.
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
|
|
for (JPH::SoftBodyVertex &vertex : physics_vertices) {
|
|
vertex.mPosition = vertex.mPreviousPosition = relative_transform.Multiply3x3(vertex.mPosition);
|
|
vertex.mVelocity = JPH::Vec3::sZero();
|
|
}
|
|
|
|
_transform_changed();
|
|
}
|
|
|
|
AABB JoltSoftBody3D::get_bounds() const {
|
|
ERR_FAIL_COND_V_MSG(!in_space(), AABB(), vformat("Failed to retrieve world bounds of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
return to_godot(jolt_body->GetWorldSpaceBounds());
|
|
}
|
|
|
|
void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
|
|
// Ideally we would emit an actual error here, but that would spam the logs to the point where the actual cause will be drowned out.
|
|
if (unlikely(!in_space())) {
|
|
return;
|
|
}
|
|
|
|
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
|
|
typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex;
|
|
typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace;
|
|
|
|
const JPH::Array<SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
const JPH::Array<SoftBodyFace> &physics_faces = motion_properties.GetFaces();
|
|
|
|
const int physics_vertex_count = (int)physics_vertices.size();
|
|
|
|
normals.clear();
|
|
normals.resize(physics_vertex_count);
|
|
|
|
// Compute vertex normals using smooth-shading:
|
|
// Each vertex should use the average normal of all faces it is a part of.
|
|
// Iterate over each face, and add the face normal to each of the face vertices.
|
|
// By the end of the loop, each vertex normal will be the sum of all face normals it belongs to.
|
|
for (const SoftBodyFace &physics_face : physics_faces) {
|
|
// Jolt uses a different winding order, so we swap the indices to account for that.
|
|
|
|
const uint32_t i0 = physics_face.mVertex[2];
|
|
const uint32_t i1 = physics_face.mVertex[1];
|
|
const uint32_t i2 = physics_face.mVertex[0];
|
|
|
|
const Vector3 v0 = to_godot(physics_vertices[i0].mPosition);
|
|
const Vector3 v1 = to_godot(physics_vertices[i1].mPosition);
|
|
const Vector3 v2 = to_godot(physics_vertices[i2].mPosition);
|
|
|
|
const Vector3 normal = (v2 - v0).cross(v1 - v0).normalized();
|
|
|
|
normals[i0] += normal;
|
|
normals[i1] += normal;
|
|
normals[i2] += normal;
|
|
}
|
|
// Normalize the vertex normals to have length 1.0
|
|
for (Vector3 &n : normals) {
|
|
real_t len = n.length();
|
|
// Some normals may have length 0 if the face was degenerate,
|
|
// so don't divide by zero.
|
|
if (len > CMP_EPSILON) {
|
|
n /= len;
|
|
}
|
|
}
|
|
|
|
const int mesh_vertex_count = mesh_to_physics.size();
|
|
const JPH::RVec3 body_position = jolt_body->GetCenterOfMassPosition();
|
|
|
|
for (int i = 0; i < mesh_vertex_count; ++i) {
|
|
const int physics_index = mesh_to_physics[i];
|
|
if (physics_index >= 0) {
|
|
const Vector3 vertex = to_godot(body_position + physics_vertices[(size_t)physics_index].mPosition);
|
|
const Vector3 normal = normals[(uint32_t)physics_index];
|
|
|
|
p_rendering_server_handler->set_vertex(i, vertex);
|
|
p_rendering_server_handler->set_normal(i, normal);
|
|
}
|
|
}
|
|
|
|
p_rendering_server_handler->set_aabb(get_bounds());
|
|
}
|
|
|
|
Vector3 JoltSoftBody3D::get_vertex_position(int p_index) {
|
|
ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
ERR_FAIL_INDEX_V(p_index, (int)mesh_to_physics.size(), Vector3());
|
|
const int physics_index = mesh_to_physics[p_index];
|
|
ERR_FAIL_COND_V_MSG(physics_index < 0, Vector3(), vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. Position cannot be returned.", p_index, to_string()));
|
|
|
|
const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
const JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
|
|
|
|
return to_godot(jolt_body->GetCenterOfMassPosition() + physics_vertex.mPosition);
|
|
}
|
|
|
|
void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
|
|
ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
ERR_FAIL_INDEX(p_index, (int)mesh_to_physics.size());
|
|
const int physics_index = mesh_to_physics[p_index];
|
|
ERR_FAIL_COND_MSG(physics_index < 0, vformat("Soft body vertex %d was not used by a face and has been omitted for '%s'. Position cannot be set.", p_index, to_string()));
|
|
|
|
JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*jolt_body->GetMotionPropertiesUnchecked());
|
|
JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
|
|
JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
|
|
|
|
const JPH::RVec3 center_of_mass = jolt_body->GetCenterOfMassPosition();
|
|
physics_vertex.mPosition = JPH::Vec3(to_jolt_r(p_position) - center_of_mass);
|
|
|
|
_vertices_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::pin_vertex(int p_index) {
|
|
pinned_vertices.insert(p_index);
|
|
|
|
_pins_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::unpin_vertex(int p_index) {
|
|
pinned_vertices.erase(p_index);
|
|
|
|
_pins_changed();
|
|
}
|
|
|
|
void JoltSoftBody3D::unpin_all_vertices() {
|
|
pinned_vertices.clear();
|
|
|
|
_pins_changed();
|
|
}
|
|
|
|
bool JoltSoftBody3D::is_vertex_pinned(int p_index) const {
|
|
ERR_FAIL_COND_V_MSG(!in_space(), false, vformat("Failed retrieve pin status of point for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
|
|
|
|
ERR_FAIL_INDEX_V(p_index, (int)mesh_to_physics.size(), false);
|
|
const int physics_index = mesh_to_physics[p_index];
|
|
|
|
return pinned_vertices.has(physics_index);
|
|
}
|