1/**************************************************************************/
2/* godot_soft_body_3d.h */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#ifndef GODOT_SOFT_BODY_3D_H
32#define GODOT_SOFT_BODY_3D_H
33
34#include "godot_area_3d.h"
35#include "godot_collision_object_3d.h"
36
37#include "core/math/aabb.h"
38#include "core/math/dynamic_bvh.h"
39#include "core/math/vector3.h"
40#include "core/templates/hash_set.h"
41#include "core/templates/local_vector.h"
42#include "core/templates/vset.h"
43
44class GodotConstraint3D;
45
46class GodotSoftBody3D : public GodotCollisionObject3D {
47 RID soft_mesh;
48
49 struct Node {
50 Vector3 s; // Source position
51 Vector3 x; // Position
52 Vector3 q; // Previous step position/Test position
53 Vector3 f; // Force accumulator
54 Vector3 v; // Velocity
55 Vector3 bv; // Biased Velocity
56 Vector3 n; // Normal
57 real_t area = 0.0; // Area
58 real_t im = 0.0; // 1/mass
59 DynamicBVH::ID leaf; // Leaf data
60 uint32_t index = 0;
61 };
62
63 struct Link {
64 Vector3 c3; // gradient
65 Node *n[2] = { nullptr, nullptr }; // Node pointers
66 real_t rl = 0.0; // Rest length
67 real_t c0 = 0.0; // (ima+imb)*kLST
68 real_t c1 = 0.0; // rl^2
69 real_t c2 = 0.0; // |gradient|^2/c0
70 };
71
72 struct Face {
73 Vector3 centroid;
74 Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers
75 Vector3 normal; // Normal
76 real_t ra = 0.0; // Rest area
77 DynamicBVH::ID leaf; // Leaf data
78 uint32_t index = 0;
79 };
80
81 LocalVector<Node> nodes;
82 LocalVector<Link> links;
83 LocalVector<Face> faces;
84
85 DynamicBVH node_tree;
86 DynamicBVH face_tree;
87
88 LocalVector<uint32_t> map_visual_to_physics;
89
90 AABB bounds;
91
92 real_t collision_margin = 0.05;
93
94 real_t total_mass = 1.0;
95 real_t inv_total_mass = 1.0;
96
97 int iteration_count = 5;
98 real_t linear_stiffness = 0.5; // [0,1]
99 real_t pressure_coefficient = 0.0; // [-inf,+inf]
100 real_t damping_coefficient = 0.01; // [0,1]
101 real_t drag_coefficient = 0.0; // [0,1]
102 LocalVector<int> pinned_vertices;
103
104 SelfList<GodotSoftBody3D> active_list;
105
106 HashSet<GodotConstraint3D *> constraints;
107
108 Vector<AreaCMP> areas;
109
110 VSet<RID> exceptions;
111
112 uint64_t island_step = 0;
113
114 _FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face);
115
116public:
117 GodotSoftBody3D();
118
119 const AABB &get_bounds() const { return bounds; }
120
121 void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
122 Variant get_state(PhysicsServer3D::BodyState p_state) const;
123
124 _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
125 _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
126 _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; }
127 _FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
128
129 _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
130 _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
131 _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
132 _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
133
134 _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
135 _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
136
137 _FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
138 int index = areas.find(AreaCMP(p_area));
139 if (index > -1) {
140 areas.write[index].refCount += 1;
141 } else {
142 areas.ordered_insert(AreaCMP(p_area));
143 }
144 }
145
146 _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
147 int index = areas.find(AreaCMP(p_area));
148 if (index > -1) {
149 areas.write[index].refCount -= 1;
150 if (areas[index].refCount < 1) {
151 areas.remove_at(index);
152 }
153 }
154 }
155
156 virtual void set_space(GodotSpace3D *p_space) override;
157
158 void set_mesh(RID p_mesh);
159
160 void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler);
161
162 Vector3 get_vertex_position(int p_index) const;
163 void set_vertex_position(int p_index, const Vector3 &p_position);
164
165 void pin_vertex(int p_index);
166 void unpin_vertex(int p_index);
167 void unpin_all_vertices();
168 bool is_vertex_pinned(int p_index) const;
169
170 uint32_t get_node_count() const;
171 real_t get_node_inv_mass(uint32_t p_node_index) const;
172 Vector3 get_node_position(uint32_t p_node_index) const;
173 Vector3 get_node_velocity(uint32_t p_node_index) const;
174 Vector3 get_node_biased_velocity(uint32_t p_node_index) const;
175 void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
176 void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
177
178 uint32_t get_face_count() const;
179 void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const;
180 Vector3 get_face_normal(uint32_t p_face_index) const;
181
182 void set_iteration_count(int p_val);
183 _FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; }
184
185 void set_total_mass(real_t p_val);
186 _FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
187 _FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; }
188
189 void set_collision_margin(real_t p_val);
190 _FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; }
191
192 void set_linear_stiffness(real_t p_val);
193 _FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
194
195 void set_pressure_coefficient(real_t p_val);
196 _FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
197
198 void set_damping_coefficient(real_t p_val);
199 _FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
200
201 void set_drag_coefficient(real_t p_val);
202 _FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
203
204 void predict_motion(real_t p_delta);
205 void solve_constraints(real_t p_delta);
206
207 _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return static_cast<Node *>(p_node)->index; }
208 _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return static_cast<Face *>(p_face)->index; }
209
210 // Return true to stop the query.
211 // p_index is the node index for AABB query, face index for Ray query.
212 typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata);
213
214 void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata);
215 void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata);
216
217protected:
218 virtual void _shapes_changed() override;
219
220private:
221 void update_normals_and_centroids();
222 void update_bounds();
223 void update_constants();
224 void update_area();
225 void reset_link_rest_lengths();
226 void update_link_constants();
227
228 void apply_nodes_transform(const Transform3D &p_transform);
229
230 void add_velocity(const Vector3 &p_velocity);
231
232 void apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas);
233
234 bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
235 void generate_bending_constraints(int p_distance);
236 void reoptimize_link_order();
237 void append_link(uint32_t p_node1, uint32_t p_node2);
238 void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3);
239
240 void solve_links(real_t kst, real_t ti);
241
242 void initialize_face_tree();
243 void update_face_tree(real_t p_delta);
244
245 void initialize_shape(bool p_force_move = true);
246 void deinitialize_shape();
247
248 void destroy();
249};
250
251class GodotSoftBodyShape3D : public GodotShape3D {
252 GodotSoftBody3D *soft_body = nullptr;
253
254public:
255 GodotSoftBody3D *get_soft_body() const { return soft_body; }
256
257 virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SOFT_BODY; }
258 virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { r_min = r_max = 0.0; }
259 virtual Vector3 get_support(const Vector3 &p_normal) const override { return Vector3(); }
260 virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
261
262 virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
263 virtual bool intersect_point(const Vector3 &p_point) const override;
264 virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
265 virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); }
266
267 virtual void set_data(const Variant &p_data) override {}
268 virtual Variant get_data() const override { return Variant(); }
269
270 void update_bounds();
271
272 GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body);
273 ~GodotSoftBodyShape3D() {}
274};
275
276#endif // GODOT_SOFT_BODY_3D_H
277