1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban, Jay Taves
13 // =============================================================================
14 //
15 // Deformable terrain based on SCM (Soil Contact Model) from DLR
16 // (Krenn & Hirzinger)
17 //
18 // =============================================================================
19
20 #include <cstdio>
21 #include <cmath>
22 #include <queue>
23 #include <unordered_set>
24 #include <limits>
25
26 #ifdef _OPENMP
27 #include <omp.h>
28 #endif
29
30 #include "chrono/physics/ChMaterialSurfaceNSC.h"
31 #include "chrono/physics/ChMaterialSurfaceSMC.h"
32 #include "chrono/assets/ChTexture.h"
33 #include "chrono/assets/ChBoxShape.h"
34 #include "chrono/utils/ChConvexHull.h"
35
36 #include "chrono_vehicle/ChVehicleModelData.h"
37 #include "chrono_vehicle/terrain/SCMDeformableTerrain.h"
38
39 #include "chrono_thirdparty/stb/stb.h"
40
41 namespace chrono {
42 namespace vehicle {
43
44 // -----------------------------------------------------------------------------
45 // Implementation of the SCMDeformableTerrain wrapper class
46 // -----------------------------------------------------------------------------
47
SCMDeformableTerrain(ChSystem * system,bool visualization_mesh)48 SCMDeformableTerrain::SCMDeformableTerrain(ChSystem* system, bool visualization_mesh) {
49 m_ground = chrono_types::make_shared<SCMDeformableSoil>(system, visualization_mesh);
50 system->Add(m_ground);
51 }
52
53 // Get the initial terrain height below the specified location.
GetInitHeight(const ChVector<> & loc) const54 double SCMDeformableTerrain::GetInitHeight(const ChVector<>& loc) const {
55 return m_ground->GetInitHeight(loc);
56 }
57
58 // Get the initial terrain normal at the point below the specified location.
GetInitNormal(const ChVector<> & loc) const59 ChVector<> SCMDeformableTerrain::GetInitNormal(const ChVector<>& loc) const {
60 return m_ground->GetInitNormal(loc);
61 }
62
63 // Get the terrain height below the specified location.
GetHeight(const ChVector<> & loc) const64 double SCMDeformableTerrain::GetHeight(const ChVector<>& loc) const {
65 return m_ground->GetHeight(loc);
66 }
67
68 // Get the terrain normal at the point below the specified location.
GetNormal(const ChVector<> & loc) const69 ChVector<> SCMDeformableTerrain::GetNormal(const ChVector<>& loc) const {
70 return m_ground->GetNormal(loc);
71 }
72
73 // Return the terrain coefficient of friction at the specified location.
GetCoefficientFriction(const ChVector<> & loc) const74 float SCMDeformableTerrain::GetCoefficientFriction(const ChVector<>& loc) const {
75 return m_friction_fun ? (*m_friction_fun)(loc) : 0.8f;
76 }
77
78 // Set the color of the visualization assets.
SetColor(const ChColor & color)79 void SCMDeformableTerrain::SetColor(const ChColor& color) {
80 if (m_ground->m_color)
81 m_ground->m_color->SetColor(color);
82 }
83
84 // Set the texture and texture scaling.
SetTexture(const std::string tex_file,float tex_scale_x,float tex_scale_y)85 void SCMDeformableTerrain::SetTexture(const std::string tex_file, float tex_scale_x, float tex_scale_y) {
86 std::shared_ptr<ChTexture> texture(new ChTexture);
87 texture->SetTextureFilename(tex_file);
88 texture->SetTextureScale(tex_scale_x, tex_scale_y);
89 m_ground->AddAsset(texture);
90 }
91
92 // Set the SCM reference plane.
SetPlane(const ChCoordsys<> & plane)93 void SCMDeformableTerrain::SetPlane(const ChCoordsys<>& plane) {
94 m_ground->m_plane = plane;
95 m_ground->m_Z = plane.rot.GetZaxis();
96 }
97
98 // Get the SCM reference plane.
GetPlane() const99 const ChCoordsys<>& SCMDeformableTerrain::GetPlane() const {
100 return m_ground->m_plane;
101 }
102
103 // Set the visualization mesh as wireframe or as solid.
SetMeshWireframe(bool val)104 void SCMDeformableTerrain::SetMeshWireframe(bool val) {
105 if (m_ground->m_trimesh_shape)
106 m_ground->m_trimesh_shape->SetWireframe(val);
107 }
108
109 // Get the trimesh that defines the ground shape.
GetMesh() const110 std::shared_ptr<ChTriangleMeshShape> SCMDeformableTerrain::GetMesh() const {
111 return m_ground->m_trimesh_shape;
112 }
113
114 // Save the visualization mesh as a Wavefront OBJ file.
WriteMesh(const std::string & filename) const115 void SCMDeformableTerrain::WriteMesh(const std::string& filename) const {
116 if (!m_ground->m_trimesh_shape) {
117 std::cout << "SCMDeformableTerrain::WriteMesh -- visualization mesh not created.";
118 return;
119 }
120 auto trimesh = m_ground->m_trimesh_shape->GetMesh();
121 std::vector<geometry::ChTriangleMeshConnected> meshes = {*trimesh};
122 trimesh->WriteWavefront(filename, meshes);
123 }
124
125 // Set properties of the SCM soil model.
SetSoilParameters(double Bekker_Kphi,double Bekker_Kc,double Bekker_n,double Mohr_cohesion,double Mohr_friction,double Janosi_shear,double elastic_K,double damping_R)126 void SCMDeformableTerrain::SetSoilParameters(
127 double Bekker_Kphi, // Kphi, frictional modulus in Bekker model
128 double Bekker_Kc, // Kc, cohesive modulus in Bekker model
129 double Bekker_n, // n, exponent of sinkage in Bekker model (usually 0.6...1.8)
130 double Mohr_cohesion, // Cohesion for shear failure [Pa]
131 double Mohr_friction, // Friction angle for shear failure [degree]
132 double Janosi_shear, // Shear parameter in Janosi-Hanamoto formula [m]
133 double elastic_K, // elastic stiffness K per unit area, [Pa/m] (must be larger than Kphi)
134 double damping_R // vertical damping R per unit area [Pa.s/m] (proportional to vertical speed)
135 ) {
136 m_ground->m_Bekker_Kphi = Bekker_Kphi;
137 m_ground->m_Bekker_Kc = Bekker_Kc;
138 m_ground->m_Bekker_n = Bekker_n;
139 m_ground->m_Mohr_cohesion = Mohr_cohesion;
140 m_ground->m_Mohr_mu = std::tan(Mohr_friction * CH_C_DEG_TO_RAD);
141 m_ground->m_Janosi_shear = Janosi_shear;
142 m_ground->m_elastic_K = ChMax(elastic_K, Bekker_Kphi);
143 m_ground->m_damping_R = damping_R;
144 }
145
146 // Enable/disable bulldozing effect.
EnableBulldozing(bool val)147 void SCMDeformableTerrain::EnableBulldozing(bool val) {
148 m_ground->m_bulldozing = val;
149 }
150
151 // Set parameters controlling the creation of side ruts (bulldozing effects).
SetBulldozingParameters(double erosion_angle,double flow_factor,int erosion_iterations,int erosion_propagations)152 void SCMDeformableTerrain::SetBulldozingParameters(
153 double erosion_angle, // angle of erosion of the displaced material [degrees]
154 double flow_factor, // growth of lateral volume relative to pressed volume
155 int erosion_iterations, // number of erosion refinements per timestep
156 int erosion_propagations // number of concentric vertex selections subject to erosion
157 ) {
158 m_ground->m_flow_factor = flow_factor;
159 m_ground->m_erosion_slope = std::tan(erosion_angle * CH_C_DEG_TO_RAD);
160 m_ground->m_erosion_iterations = erosion_iterations;
161 m_ground->m_erosion_propagations = erosion_propagations;
162 }
163
SetTestHeight(double offset)164 void SCMDeformableTerrain::SetTestHeight(double offset) {
165 m_ground->m_test_offset_up = offset;
166 }
167
GetTestHeight() const168 double SCMDeformableTerrain::GetTestHeight() const {
169 return m_ground->m_test_offset_up;
170 }
171
172 // Set the color plot type.
SetPlotType(DataPlotType plot_type,double min_val,double max_val)173 void SCMDeformableTerrain::SetPlotType(DataPlotType plot_type, double min_val, double max_val) {
174 m_ground->m_plot_type = plot_type;
175 m_ground->m_plot_v_min = min_val;
176 m_ground->m_plot_v_max = max_val;
177 }
178
179 // Enable moving patch.
AddMovingPatch(std::shared_ptr<ChBody> body,const ChVector<> & OOBB_center,const ChVector<> & OOBB_dims)180 void SCMDeformableTerrain::AddMovingPatch(std::shared_ptr<ChBody> body,
181 const ChVector<>& OOBB_center,
182 const ChVector<>& OOBB_dims) {
183 SCMDeformableSoil::MovingPatchInfo pinfo;
184 pinfo.m_body = body;
185 pinfo.m_center = OOBB_center;
186 pinfo.m_hdims = OOBB_dims / 2;
187
188 m_ground->m_patches.push_back(pinfo);
189
190 // Moving patch monitoring is now enabled
191 m_ground->m_moving_patch = true;
192 }
193
194 // Set user-supplied callback for evaluating location-dependent soil parameters.
RegisterSoilParametersCallback(std::shared_ptr<SoilParametersCallback> cb)195 void SCMDeformableTerrain::RegisterSoilParametersCallback(std::shared_ptr<SoilParametersCallback> cb) {
196 m_ground->m_soil_fun = cb;
197 }
198
199 // Initialize the terrain as a flat grid.
Initialize(double sizeX,double sizeY,double delta)200 void SCMDeformableTerrain::Initialize(double sizeX, double sizeY, double delta) {
201 m_ground->Initialize(sizeX, sizeY, delta);
202 }
203
204 // Initialize the terrain from a specified height map.
Initialize(const std::string & heightmap_file,double sizeX,double sizeY,double hMin,double hMax,double delta)205 void SCMDeformableTerrain::Initialize(const std::string& heightmap_file,
206 double sizeX,
207 double sizeY,
208 double hMin,
209 double hMax,
210 double delta) {
211 m_ground->Initialize(heightmap_file, sizeX, sizeY, hMin, hMax, delta);
212 }
213
214 // Get the heights of modified grid nodes.
GetModifiedNodes(bool all_nodes) const215 std::vector<SCMDeformableTerrain::NodeLevel> SCMDeformableTerrain::GetModifiedNodes(bool all_nodes) const {
216 return m_ground->GetModifiedNodes(all_nodes);
217 }
218
219 // Modify the level of grid nodes from the given list.
SetModifiedNodes(const std::vector<NodeLevel> & nodes)220 void SCMDeformableTerrain::SetModifiedNodes(const std::vector<NodeLevel>& nodes) {
221 m_ground->SetModifiedNodes(nodes);
222 }
223
224 // Return the current cumulative contact force on the specified body (due to interaction with the SCM terrain).
GetContactForce(std::shared_ptr<ChBody> body) const225 TerrainForce SCMDeformableTerrain::GetContactForce(std::shared_ptr<ChBody> body) const {
226 auto itr = m_ground->m_contact_forces.find(body.get());
227 if (itr != m_ground->m_contact_forces.end())
228 return itr->second;
229
230 TerrainForce frc;
231 frc.point = body->GetPos();
232 frc.force = ChVector<>(0, 0, 0);
233 frc.moment = ChVector<>(0, 0, 0);
234 return frc;
235 }
236
237 // Return the number of rays cast at last step.
GetNumRayCasts() const238 int SCMDeformableTerrain::GetNumRayCasts() const {
239 return m_ground->m_num_ray_casts;
240 }
241
242 // Return the number of ray hits at last step.
GetNumRayHits() const243 int SCMDeformableTerrain::GetNumRayHits() const {
244 return m_ground->m_num_ray_hits;
245 }
246
247 // Return the number of contact patches at last step.
GetNumContactPatches() const248 int SCMDeformableTerrain::GetNumContactPatches() const {
249 return m_ground->m_num_contact_patches;
250 }
251
252 // Return the number of nodes in the erosion domain at last step (bulldosing effects).
GetNumErosionNodes() const253 int SCMDeformableTerrain::GetNumErosionNodes() const {
254 return m_ground->m_num_erosion_nodes;
255 }
256
257 // Timer information
GetTimerMovingPatches() const258 double SCMDeformableTerrain::GetTimerMovingPatches() const {
259 return 1e3 * m_ground->m_timer_moving_patches();
260 }
GetTimerRayTesting() const261 double SCMDeformableTerrain::GetTimerRayTesting() const {
262 return 1e3 * m_ground->m_timer_ray_testing();
263 }
GetTimerRayCasting() const264 double SCMDeformableTerrain::GetTimerRayCasting() const {
265 return 1e3 * m_ground->m_timer_ray_casting();
266 }
GetTimerContactPatches() const267 double SCMDeformableTerrain::GetTimerContactPatches() const {
268 return 1e3 * m_ground->m_timer_contact_patches();
269 }
GetTimerContactForces() const270 double SCMDeformableTerrain::GetTimerContactForces() const {
271 return 1e3 * m_ground->m_timer_contact_forces();
272 }
GetTimerBulldozing() const273 double SCMDeformableTerrain::GetTimerBulldozing() const {
274 return 1e3 * m_ground->m_timer_bulldozing();
275 }
GetTimerVisUpdate() const276 double SCMDeformableTerrain::GetTimerVisUpdate() const {
277 return 1e3 * m_ground->m_timer_visualization();
278 }
279
280 // Print timing and counter information for last step.
PrintStepStatistics(std::ostream & os) const281 void SCMDeformableTerrain::PrintStepStatistics(std::ostream& os) const {
282 os << " Timers (ms):" << std::endl;
283 os << " Moving patches: " << 1e3 * m_ground->m_timer_moving_patches() << std::endl;
284 os << " Ray testing: " << 1e3 * m_ground->m_timer_ray_testing() << std::endl;
285 os << " Ray casting: " << 1e3 * m_ground->m_timer_ray_casting() << std::endl;
286 os << " Contact patches: " << 1e3 * m_ground->m_timer_contact_patches() << std::endl;
287 os << " Contact forces: " << 1e3 * m_ground->m_timer_contact_forces() << std::endl;
288 os << " Bulldozing: " << 1e3 * m_ground->m_timer_bulldozing() << std::endl;
289 os << " Raise boundary: " << 1e3 * m_ground->m_timer_bulldozing_boundary() << std::endl;
290 os << " Compute domain: " << 1e3 * m_ground->m_timer_bulldozing_domain() << std::endl;
291 os << " Apply erosion: " << 1e3 * m_ground->m_timer_bulldozing_erosion() << std::endl;
292 os << " Visualization: " << 1e3 * m_ground->m_timer_visualization() << std::endl;
293
294 os << " Counters:" << std::endl;
295 os << " Number ray casts: " << m_ground->m_num_ray_casts << std::endl;
296 os << " Number ray hits: " << m_ground->m_num_ray_hits << std::endl;
297 os << " Number contact patches: " << m_ground->m_num_contact_patches << std::endl;
298 os << " Number erosion nodes: " << m_ground->m_num_erosion_nodes << std::endl;
299 }
300
301 // -----------------------------------------------------------------------------
302 // Contactable user-data (contactable-soil parameters)
303 // -----------------------------------------------------------------------------
304
SCMContactableData(double area_ratio,double Mohr_cohesion,double Mohr_friction,double Janosi_shear)305 SCMContactableData::SCMContactableData(double area_ratio,
306 double Mohr_cohesion,
307 double Mohr_friction,
308 double Janosi_shear)
309 : area_ratio(area_ratio),
310 Mohr_cohesion(Mohr_cohesion),
311 Mohr_mu(std::tan(Mohr_friction * CH_C_DEG_TO_RAD)),
312 Janosi_shear(Janosi_shear) {}
313
314 // -----------------------------------------------------------------------------
315 // Implementation of SCMDeformableSoil
316 // -----------------------------------------------------------------------------
317
318 // Constructor.
SCMDeformableSoil(ChSystem * system,bool visualization_mesh)319 SCMDeformableSoil::SCMDeformableSoil(ChSystem* system, bool visualization_mesh) : m_soil_fun(nullptr) {
320 this->SetSystem(system);
321
322 if (visualization_mesh) {
323 // Create the visualization mesh and asset
324 m_trimesh_shape = std::shared_ptr<ChTriangleMeshShape>(new ChTriangleMeshShape);
325 m_trimesh_shape->SetWireframe(true);
326 m_trimesh_shape->SetFixedConnectivity();
327 this->AddAsset(m_trimesh_shape);
328
329 // Create the default mesh asset
330 m_color = std::shared_ptr<ChColorAsset>(new ChColorAsset);
331 m_color->SetColor(ChColor(0.3f, 0.3f, 0.3f));
332 this->AddAsset(m_color);
333 }
334
335 // Default SCM plane and plane normal
336 m_plane = ChCoordsys<>(VNULL, QUNIT);
337 m_Z = m_plane.rot.GetZaxis();
338
339 // Bulldozing effects
340 m_bulldozing = false;
341 m_flow_factor = 1.2;
342 m_erosion_slope = std::tan(40.0 * CH_C_DEG_TO_RAD);
343 m_erosion_iterations = 3;
344 m_erosion_propagations = 10;
345
346 // Default soil parameters
347 m_Bekker_Kphi = 2e6;
348 m_Bekker_Kc = 0;
349 m_Bekker_n = 1.1;
350 m_Mohr_cohesion = 50;
351 m_Mohr_mu = std::tan(20.0 * CH_C_DEG_TO_RAD);
352 m_Janosi_shear = 0.01;
353 m_elastic_K = 50000000;
354 m_damping_R = 0;
355
356 m_plot_type = SCMDeformableTerrain::PLOT_NONE;
357 m_plot_v_min = 0;
358 m_plot_v_max = 0.2;
359
360 m_test_offset_up = 0.1;
361 m_test_offset_down = 0.5;
362
363 m_moving_patch = false;
364 }
365
366 // Initialize the terrain as a flat grid
Initialize(double sizeX,double sizeY,double delta)367 void SCMDeformableSoil::Initialize(double sizeX, double sizeY, double delta) {
368 m_type = PatchType::FLAT;
369
370 m_nx = static_cast<int>(std::ceil((sizeX / 2) / delta)); // half number of divisions in X direction
371 m_ny = static_cast<int>(std::ceil((sizeY / 2) / delta)); // number of divisions in Y direction
372
373 m_delta = sizeX / (2 * m_nx); // grid spacing
374 m_area = std::pow(m_delta, 2); // area of a cell
375
376 // Return now if no visualization
377 if (!m_trimesh_shape)
378 return;
379
380 int nvx = 2 * m_nx + 1; // number of grid vertices in X direction
381 int nvy = 2 * m_ny + 1; // number of grid vertices in Y direction
382 int n_verts = nvx * nvy; // total number of vertices for initial visualization trimesh
383 int n_faces = 2 * (2 * m_nx) * (2 * m_ny); // total number of faces for initial visualization trimesh
384 double x_scale = 0.5 / m_nx; // scale for texture coordinates (U direction)
385 double y_scale = 0.5 / m_ny; // scale for texture coordinates (V direction)
386
387 // Readability aliases
388 auto trimesh = m_trimesh_shape->GetMesh();
389 trimesh->Clear();
390 std::vector<ChVector<>>& vertices = trimesh->getCoordsVertices();
391 std::vector<ChVector<>>& normals = trimesh->getCoordsNormals();
392 std::vector<ChVector<int>>& idx_vertices = trimesh->getIndicesVertexes();
393 std::vector<ChVector<int>>& idx_normals = trimesh->getIndicesNormals();
394 std::vector<ChVector<>>& uv_coords = trimesh->getCoordsUV();
395 std::vector<ChVector<float>>& colors = trimesh->getCoordsColors();
396
397 // Resize mesh arrays
398 vertices.resize(n_verts);
399 normals.resize(n_verts);
400 uv_coords.resize(n_verts);
401 colors.resize(n_verts);
402 idx_vertices.resize(n_faces);
403 idx_normals.resize(n_faces);
404
405 // Load mesh vertices.
406 // We order the vertices starting at the bottom-left corner, row after row.
407 // The bottom-left corner corresponds to the point (-sizeX/2, -sizeY/2).
408 // UV coordinates are mapped in [0,1] x [0,1].
409 int iv = 0;
410 for (int iy = 0; iy < nvy; iy++) {
411 double y = iy * m_delta - 0.5 * sizeY;
412 for (int ix = 0; ix < nvx; ix++) {
413 double x = ix * m_delta - 0.5 * sizeX;
414 // Set vertex location
415 vertices[iv] = m_plane * ChVector<>(x, y, 0);
416 // Initialize vertex normal to Y up
417 normals[iv] = m_plane.TransformDirectionLocalToParent(ChVector<>(0, 0, 1));
418 // Assign color white to all vertices
419 colors[iv] = ChVector<float>(1, 1, 1);
420 // Set UV coordinates in [0,1] x [0,1]
421 uv_coords[iv] = ChVector<>(ix * x_scale, iy * y_scale, 0.0);
422 ++iv;
423 }
424 }
425
426 // Specify triangular faces (two at a time).
427 // Specify the face vertices counter-clockwise.
428 // Set the normal indices same as the vertex indices.
429 int it = 0;
430 for (int iy = 0; iy < nvy - 1; iy++) {
431 for (int ix = 0; ix < nvx - 1; ix++) {
432 int v0 = ix + nvx * iy;
433 idx_vertices[it] = ChVector<int>(v0, v0 + 1, v0 + nvx + 1);
434 idx_normals[it] = ChVector<int>(v0, v0 + 1, v0 + nvx + 1);
435 ++it;
436 idx_vertices[it] = ChVector<int>(v0, v0 + nvx + 1, v0 + nvx);
437 idx_normals[it] = ChVector<int>(v0, v0 + nvx + 1, v0 + nvx);
438 ++it;
439 }
440 }
441 }
442
443 // Initialize the terrain from a specified height map.
Initialize(const std::string & heightmap_file,double sizeX,double sizeY,double hMin,double hMax,double delta)444 void SCMDeformableSoil::Initialize(const std::string& heightmap_file,
445 double sizeX,
446 double sizeY,
447 double hMin,
448 double hMax,
449 double delta) {
450 m_type = PatchType::HEIGHT_MAP;
451
452 // Read the image file (request only 1 channel) and extract number of pixels.
453 STB hmap;
454 if (!hmap.ReadFromFile(heightmap_file, 1)) {
455 std::cout << "STB error in reading height map file " << heightmap_file << std::endl;
456 throw ChException("Cannot read height map image file");
457 }
458 int nx_img = hmap.GetWidth();
459 int ny_img = hmap.GetHeight();
460
461 double dx_img = 1.0 / (nx_img - 1.0);
462 double dy_img = 1.0 / (ny_img - 1.0);
463
464 m_nx = static_cast<int>(std::ceil((sizeX / 2) / delta)); // half number of divisions in X direction
465 m_ny = static_cast<int>(std::ceil((sizeY / 2) / delta)); // number of divisions in Y direction
466
467 m_delta = sizeX / (2.0 * m_nx); // grid spacing
468 m_area = std::pow(m_delta, 2); // area of a cell
469
470 double dx_grid = 0.5 / m_nx;
471 double dy_grid = 0.5 / m_ny;
472
473 int nvx = 2 * m_nx + 1; // number of grid vertices in X direction
474 int nvy = 2 * m_ny + 1; // number of grid vertices in Y direction
475 int n_verts = nvx * nvy; // total number of vertices for initial visualization trimesh
476 int n_faces = 2 * (2 * m_nx) * (2 * m_ny); // total number of faces for initial visualization trimesh
477 double x_scale = 0.5 / m_nx; // scale for texture coordinates (U direction)
478 double y_scale = 0.5 / m_ny; // scale for texture coordinates (V direction)
479
480 // Resample image and calculate interpolated gray levels and then map it to the height range, with black
481 // corresponding to hMin and white corresponding to hMax. Entry (0,0) corresponds to bottom-left grid vertex.
482 // Note that pixels in the image start at top-left corner.
483 double h_scale = (hMax - hMin) / hmap.GetRange();
484 m_heights = ChMatrixDynamic<>(nvx, nvy);
485 for (int ix = 0; ix < nvx; ix++) {
486 double x = ix * dx_grid; // x location in image (in [0,1], 0 at left)
487 int jx1 = (int)std::floor(x / dx_img); // Left pixel
488 int jx2 = (int)std::ceil(x / dx_img); // Right pixel
489 double ax = (x - jx1 * dx_img) / dx_img; // Scaled offset from left pixel
490
491 assert(ax < 1.0);
492 assert(jx1 < nx_img);
493 assert(jx2 < nx_img);
494 assert(jx1 <= jx2);
495
496 for (int iy = 0; iy < nvy; iy++) {
497 double y = (2 * m_ny - iy) * dy_grid; // y location in image (in [0,1], 0 at top)
498 int jy1 = (int)std::floor(y / dy_img); // Up pixel
499 int jy2 = (int)std::ceil(y / dy_img); // Down pixel
500 double ay = (y - jy1 * dy_img) / dy_img; // Scaled offset from down pixel
501
502 assert(ay < 1.0);
503 assert(jy1 < ny_img);
504 assert(jy2 < ny_img);
505 assert(jy1 <= jy2);
506
507 // Gray levels at left-up, left-down, right-up, and right-down pixels
508 double g11 = hmap.Gray(jx1, jy1);
509 double g12 = hmap.Gray(jx1, jy2);
510 double g21 = hmap.Gray(jx2, jy1);
511 double g22 = hmap.Gray(jx2, jy2);
512
513 // Bilinear interpolation (gray level)
514 m_heights(ix, iy) = (1 - ax) * (1 - ay) * g11 + (1 - ax) * ay * g12 + ax * (1 - ay) * g21 + ax * ay * g22;
515 // Map into height range
516 m_heights(ix, iy) = hMin + m_heights(ix, iy) * h_scale;
517 }
518 }
519
520 // Return now if no visualization
521 if (!m_trimesh_shape)
522 return;
523
524 // Readability aliases
525 auto trimesh = m_trimesh_shape->GetMesh();
526 trimesh->Clear();
527 std::vector<ChVector<>>& vertices = trimesh->getCoordsVertices();
528 std::vector<ChVector<>>& normals = trimesh->getCoordsNormals();
529 std::vector<ChVector<int>>& idx_vertices = trimesh->getIndicesVertexes();
530 std::vector<ChVector<int>>& idx_normals = trimesh->getIndicesNormals();
531 std::vector<ChVector<>>& uv_coords = trimesh->getCoordsUV();
532 std::vector<ChVector<float>>& colors = trimesh->getCoordsColors();
533
534 // Resize mesh arrays.
535 vertices.resize(n_verts);
536 normals.resize(n_verts);
537 uv_coords.resize(n_verts);
538 colors.resize(n_verts);
539 idx_vertices.resize(n_faces);
540 idx_normals.resize(n_faces);
541
542 // Load mesh vertices.
543 // We order the vertices starting at the bottom-left corner, row after row.
544 // The bottom-left corner corresponds to the point (-sizeX/2, -sizeY/2).
545 // UV coordinates are mapped in [0,1] x [0,1]. Use smoothed vertex normals.
546 int iv = 0;
547 for (int iy = 0; iy < nvy; iy++) {
548 double y = iy * m_delta - 0.5 * sizeY;
549 for (int ix = 0; ix < nvx; ix++) {
550 double x = ix * m_delta - 0.5 * sizeX;
551 // Set vertex location
552 vertices[iv] = m_plane * ChVector<>(x, y, m_heights(ix, iy));
553 // Initialize vertex normal to Y up
554 normals[iv] = m_plane.TransformDirectionLocalToParent(ChVector<>(0, 0, 1));
555 // Assign color white to all vertices
556 colors[iv] = ChVector<float>(1, 1, 1);
557 // Set UV coordinates in [0,1] x [0,1]
558 uv_coords[iv] = ChVector<>(ix * x_scale, iy * y_scale, 0.0);
559 ++iv;
560 }
561 }
562
563 // Specify triangular faces (two at a time).
564 // Specify the face vertices counter-clockwise.
565 // Set the normal indices same as the vertex indices.
566 int it = 0;
567 for (int iy = 0; iy < nvy - 1; iy++) {
568 for (int ix = 0; ix < nvx - 1; ix++) {
569 int v0 = ix + nvx * iy;
570 idx_vertices[it] = ChVector<int>(v0, v0 + 1, v0 + nvx + 1);
571 idx_normals[it] = ChVector<int>(v0, v0 + 1, v0 + nvx + 1);
572 ++it;
573 idx_vertices[it] = ChVector<int>(v0, v0 + nvx + 1, v0 + nvx);
574 idx_normals[it] = ChVector<int>(v0, v0 + nvx + 1, v0 + nvx);
575 ++it;
576 }
577 }
578
579 // Initialize the array of accumulators (number of adjacent faces to a vertex)
580 std::vector<int> accumulators(n_verts, 0);
581
582 // Calculate normals and then average the normals from all adjacent faces.
583 for (it = 0; it < n_faces; it++) {
584 // Calculate the triangle normal as a normalized cross product.
585 ChVector<> nrm = Vcross(vertices[idx_vertices[it][1]] - vertices[idx_vertices[it][0]],
586 vertices[idx_vertices[it][2]] - vertices[idx_vertices[it][0]]);
587 nrm.Normalize();
588 // Increment the normals of all incident vertices by the face normal
589 normals[idx_normals[it][0]] += nrm;
590 normals[idx_normals[it][1]] += nrm;
591 normals[idx_normals[it][2]] += nrm;
592 // Increment the count of all incident vertices by 1
593 accumulators[idx_normals[it][0]] += 1;
594 accumulators[idx_normals[it][1]] += 1;
595 accumulators[idx_normals[it][2]] += 1;
596 }
597
598 // Set the normals to the average values.
599 for (int in = 0; in < n_verts; in++) {
600 normals[in] /= (double)accumulators[in];
601 }
602 }
603
SetupInitial()604 void SCMDeformableSoil::SetupInitial() {
605 // If no user-specified moving patches, create one that will encompass all collision shapes in the system
606 if (!m_moving_patch) {
607 SCMDeformableSoil::MovingPatchInfo pinfo;
608 pinfo.m_body = nullptr;
609 m_patches.push_back(pinfo);
610 }
611 }
612
CheckMeshBounds(const ChVector2<int> & loc) const613 bool SCMDeformableSoil::CheckMeshBounds(const ChVector2<int>& loc) const {
614 return loc.x() >= -m_nx && loc.x() <= m_nx && loc.y() >= -m_ny && loc.y() <= m_ny;
615 }
616
617 // Get index of trimesh vertex corresponding to the specified grid vertex.
GetMeshVertexIndex(const ChVector2<int> & loc)618 int SCMDeformableSoil::GetMeshVertexIndex(const ChVector2<int>& loc) {
619 assert(loc.x() >= -m_nx);
620 assert(loc.x() <= +m_nx);
621 assert(loc.y() >= -m_ny);
622 assert(loc.y() <= +m_ny);
623 return (loc.x() + m_nx) + (2 * m_nx + 1) * (loc.y() + m_ny);
624 }
625
626 // Get indices of trimesh faces incident to the specified grid vertex.
GetMeshFaceIndices(const ChVector2<int> & loc)627 std::vector<int> SCMDeformableSoil::GetMeshFaceIndices(const ChVector2<int>& loc) {
628 int i = loc.x();
629 int j = loc.y();
630
631 // Ignore boundary vertices
632 if (i == -m_nx || i == m_nx || j == -m_ny || j == m_ny)
633 return std::vector<int>();
634
635 // Load indices of 6 adjacent faces
636 i += m_nx;
637 j += m_ny;
638 int nx = 2 * m_nx;
639 std::vector<int> faces(6);
640 faces[0] = 2 * ((i - 1) + nx * (j - 1));
641 faces[1] = 2 * ((i - 1) + nx * (j - 1)) + 1;
642 faces[2] = 2 * ((i - 1) + nx * (j - 0));
643 faces[3] = 2 * ((i - 0) + nx * (j - 0));
644 faces[4] = 2 * ((i - 0) + nx * (j - 0)) + 1;
645 faces[5] = 2 * ((i - 0) + nx * (j - 1)) + 1;
646
647 return faces;
648 }
649
650 // Get the initial undeformed terrain height (relative to the SCM plane) at the specified grid vertex.
GetInitHeight(const ChVector2<int> & loc) const651 double SCMDeformableSoil::GetInitHeight(const ChVector2<int>& loc) const {
652 switch (m_type) {
653 case PatchType::FLAT:
654 return 0;
655 case PatchType::HEIGHT_MAP: {
656 auto x = ChClamp(loc.x(), -m_nx, +m_nx);
657 auto y = ChClamp(loc.y(), -m_ny, +m_ny);
658 return m_heights(x + m_nx, y + m_ny);
659 }
660 default:
661 return 0;
662 }
663 }
664
665 // Get the initial undeformed terrain normal (relative to the SCM plane) at the specified grid node.
GetInitNormal(const ChVector2<int> & loc) const666 ChVector<> SCMDeformableSoil::GetInitNormal(const ChVector2<int>& loc) const {
667 switch (m_type) {
668 case PatchType::HEIGHT_MAP: {
669 // Average normals of 4 triangular faces incident to given grid node
670 auto hE = GetInitHeight(loc + ChVector2<int>(1, 0)); // east
671 auto hW = GetInitHeight(loc - ChVector2<int>(1, 0)); // west
672 auto hN = GetInitHeight(loc + ChVector2<int>(0, 1)); // north
673 auto hS = GetInitHeight(loc - ChVector2<int>(0, 1)); // south
674 return ChVector<>(hW - hE, hS - hN, 2 * m_delta).GetNormalized();
675 }
676 case PatchType::FLAT:
677 default:
678 return ChVector<>(0, 0, 1);
679 }
680 }
681
682 // Get the terrain height (relative to the SCM plane) at the specified grid vertex.
GetHeight(const ChVector2<int> & loc) const683 double SCMDeformableSoil::GetHeight(const ChVector2<int>& loc) const {
684 // First query the hash-map
685 auto p = m_grid_map.find(loc);
686 if (p != m_grid_map.end())
687 return p->second.level;
688
689 // Else return undeformed height
690 return GetInitHeight(loc);
691 }
692
693 // Get the terrain normal (relative to the SCM plane) at the specified grid vertex.
GetNormal(const ChVector2<> & loc) const694 ChVector<> SCMDeformableSoil::GetNormal(const ChVector2<>& loc) const {
695 switch (m_type) {
696 case PatchType::HEIGHT_MAP: {
697 // Average normals of 4 triangular faces incident to given grid node
698 auto hE = GetHeight(loc + ChVector2<int>(1, 0)); // east
699 auto hW = GetHeight(loc - ChVector2<int>(1, 0)); // west
700 auto hN = GetHeight(loc + ChVector2<int>(0, 1)); // north
701 auto hS = GetHeight(loc - ChVector2<int>(0, 1)); // south
702 return ChVector<>(hW - hE, hS - hN, 2 * m_delta).GetNormalized();
703 }
704 case PatchType::FLAT:
705 default:
706 return ChVector<>(0, 0, 1);
707 }
708 }
709
710 // Get the initial terrain height below the specified location.
GetInitHeight(const ChVector<> & loc) const711 double SCMDeformableSoil::GetInitHeight(const ChVector<>& loc) const {
712 // Express location in the SCM frame
713 ChVector<> loc_loc = m_plane.TransformPointParentToLocal(loc);
714
715 // Get height (relative to SCM plane) at closest grid vertex (approximation)
716 int i = static_cast<int>(std::round(loc_loc.x() / m_delta));
717 int j = static_cast<int>(std::round(loc_loc.y() / m_delta));
718 loc_loc.z() = GetInitHeight(ChVector2<int>(i, j));
719
720 // Express in global frame
721 ChVector<> loc_abs = m_plane.TransformPointLocalToParent(loc_loc);
722 return ChWorldFrame::Height(loc_abs);
723 }
724
725 // Get the initial terrain normal at the point below the specified location.
GetInitNormal(const ChVector<> & loc) const726 ChVector<> SCMDeformableSoil::GetInitNormal(const ChVector<>& loc) const {
727 // Express location in the SCM frame
728 ChVector<> loc_loc = m_plane.TransformPointParentToLocal(loc);
729
730 // Get height (relative to SCM plane) at closest grid vertex (approximation)
731 int i = static_cast<int>(std::round(loc_loc.x() / m_delta));
732 int j = static_cast<int>(std::round(loc_loc.y() / m_delta));
733 auto nrm_loc = GetInitNormal(ChVector2<int>(i, j));
734
735 // Express in global frame
736 auto nrm_abs = m_plane.TransformDirectionLocalToParent(nrm_loc);
737 return ChWorldFrame::FromISO(nrm_abs);
738 }
739
740 // Get the terrain height below the specified location.
GetHeight(const ChVector<> & loc) const741 double SCMDeformableSoil::GetHeight(const ChVector<>& loc) const {
742 // Express location in the SCM frame
743 ChVector<> loc_loc = m_plane.TransformPointParentToLocal(loc);
744
745 // Get height (relative to SCM plane) at closest grid vertex (approximation)
746 int i = static_cast<int>(std::round(loc_loc.x() / m_delta));
747 int j = static_cast<int>(std::round(loc_loc.y() / m_delta));
748 loc_loc.z() = GetHeight(ChVector2<int>(i, j));
749
750 // Express in global frame
751 ChVector<> loc_abs = m_plane.TransformPointLocalToParent(loc_loc);
752 return ChWorldFrame::Height(loc_abs);
753 }
754
755 // Get the terrain normal at the point below the specified location.
GetNormal(const ChVector<> & loc) const756 ChVector<> SCMDeformableSoil::GetNormal(const ChVector<>& loc) const {
757 // Express location in the SCM frame
758 ChVector<> loc_loc = m_plane.TransformPointParentToLocal(loc);
759
760 // Get height (relative to SCM plane) at closest grid vertex (approximation)
761 int i = static_cast<int>(std::round(loc_loc.x() / m_delta));
762 int j = static_cast<int>(std::round(loc_loc.y() / m_delta));
763 auto nrm_loc = GetNormal(ChVector2<int>(i, j));
764
765 // Express in global frame
766 auto nrm_abs = m_plane.TransformDirectionLocalToParent(nrm_loc);
767 return ChWorldFrame::FromISO(nrm_abs);
768 }
769
770 // Synchronize information for a moving patch
UpdateMovingPatch(MovingPatchInfo & p,const ChVector<> & Z)771 void SCMDeformableSoil::UpdateMovingPatch(MovingPatchInfo& p, const ChVector<>& Z) {
772 ChVector2<> p_min(+std::numeric_limits<double>::max());
773 ChVector2<> p_max(-std::numeric_limits<double>::max());
774
775 // Loop over all corners of the OOBB
776 for (int j = 0; j < 8; j++) {
777 int ix = j % 2;
778 int iy = (j / 2) % 2;
779 int iz = (j / 4);
780
781 // OOBB corner in body frame
782 ChVector<> c_body = p.m_center + p.m_hdims * ChVector<>(2.0 * ix - 1, 2.0 * iy - 1, 2.0 * iz - 1);
783 // OOBB corner in absolute frame
784 ChVector<> c_abs = p.m_body->GetFrame_REF_to_abs().TransformPointLocalToParent(c_body);
785 // OOBB corner expressed in SCM frame
786 ChVector<> c_scm = m_plane.TransformPointParentToLocal(c_abs);
787
788 // Update AABB of patch projection onto SCM plane
789 p_min.x() = std::min(p_min.x(), c_scm.x());
790 p_min.y() = std::min(p_min.y(), c_scm.y());
791 p_max.x() = std::max(p_max.x(), c_scm.x());
792 p_max.y() = std::max(p_max.y(), c_scm.y());
793 }
794
795 // Find index ranges for grid vertices contained in the patch projection AABB
796 int x_min = static_cast<int>(std::ceil(p_min.x() / m_delta));
797 int y_min = static_cast<int>(std::ceil(p_min.y() / m_delta));
798 int x_max = static_cast<int>(std::floor(p_max.x() / m_delta));
799 int y_max = static_cast<int>(std::floor(p_max.y() / m_delta));
800 int n_x = x_max - x_min + 1;
801 int n_y = y_max - y_min + 1;
802
803 p.m_range.resize(n_x * n_y);
804 for (int i = 0; i < n_x; i++) {
805 for (int j = 0; j < n_y; j++) {
806 p.m_range[j * n_x + i] = ChVector2<int>(i + x_min, j + y_min);
807 }
808 }
809
810 // Calculate inverse of SCM normal expressed in body frame (for optimization of ray-OBB test)
811 ChVector<> dir = p.m_body->TransformDirectionParentToLocal(Z);
812 p.m_ooN.x() = (dir.x() == 0) ? 1e10 : 1.0 / dir.x();
813 p.m_ooN.y() = (dir.y() == 0) ? 1e10 : 1.0 / dir.y();
814 p.m_ooN.z() = (dir.z() == 0) ? 1e10 : 1.0 / dir.z();
815 }
816
817 // Synchronize information for fixed patch
UpdateFixedPatch(MovingPatchInfo & p)818 void SCMDeformableSoil::UpdateFixedPatch(MovingPatchInfo& p) {
819 ChVector2<> p_min(+std::numeric_limits<double>::max());
820 ChVector2<> p_max(-std::numeric_limits<double>::max());
821
822 // Get current bounding box (AABB) of all collision shapes
823 ChVector<> aabb_min;
824 ChVector<> aabb_max;
825 GetSystem()->GetCollisionSystem()->GetBoundingBox(aabb_min, aabb_max);
826
827 // Loop over all corners of the AABB
828 for (int j = 0; j < 8; j++) {
829 int ix = j % 2;
830 int iy = (j / 2) % 2;
831 int iz = (j / 4);
832
833 // AABB corner in absolute frame
834 ChVector<> c_abs = aabb_max * ChVector<>(ix, iy, iz) + aabb_min * ChVector<>(1.0 - ix, 1.0 - iy, 1.0 - iz);
835 // AABB corner in SCM frame
836 ChVector<> c_scm = m_plane.TransformPointParentToLocal(c_abs);
837
838 // Update AABB of patch projection onto SCM plane
839 p_min.x() = std::min(p_min.x(), c_scm.x());
840 p_min.y() = std::min(p_min.y(), c_scm.y());
841 p_max.x() = std::max(p_max.x(), c_scm.x());
842 p_max.y() = std::max(p_max.y(), c_scm.y());
843 }
844
845 // Find index ranges for grid vertices contained in the patch projection AABB
846 int x_min = static_cast<int>(std::ceil(p_min.x() / m_delta));
847 int y_min = static_cast<int>(std::ceil(p_min.y() / m_delta));
848 int x_max = static_cast<int>(std::floor(p_max.x() / m_delta));
849 int y_max = static_cast<int>(std::floor(p_max.y() / m_delta));
850 int n_x = x_max - x_min + 1;
851 int n_y = y_max - y_min + 1;
852
853 p.m_range.resize(n_x * n_y);
854 for (int i = 0; i < n_x; i++) {
855 for (int j = 0; j < n_y; j++) {
856 p.m_range[j * n_x + i] = ChVector2<int>(i + x_min, j + y_min);
857 }
858 }
859 }
860
861 // Ray-OBB intersection test
RayOBBtest(const MovingPatchInfo & p,const ChVector<> & from,const ChVector<> & Z)862 bool SCMDeformableSoil::RayOBBtest(const MovingPatchInfo& p, const ChVector<>& from, const ChVector<>& Z) {
863 // Express ray origin in OBB frame
864 ChVector<> orig = p.m_body->TransformPointParentToLocal(from) - p.m_center;
865
866 // Perform ray-AABB test (slab tests)
867 double t1 = (-p.m_hdims.x() - orig.x()) * p.m_ooN.x();
868 double t2 = (+p.m_hdims.x() - orig.x()) * p.m_ooN.x();
869 double t3 = (-p.m_hdims.y() - orig.y()) * p.m_ooN.y();
870 double t4 = (+p.m_hdims.y() - orig.y()) * p.m_ooN.y();
871 double t5 = (-p.m_hdims.z() - orig.z()) * p.m_ooN.z();
872 double t6 = (+p.m_hdims.z() - orig.z()) * p.m_ooN.z();
873
874 double tmin = std::max(std::max(std::min(t1, t2), std::min(t3, t4)), std::min(t5, t6));
875 double tmax = std::min(std::min(std::max(t1, t2), std::max(t3, t4)), std::max(t5, t6));
876
877 if (tmax < 0)
878 return false;
879 if (tmin > tmax)
880 return false;
881 return true;
882 }
883
884 // Offsets for the 8 neighbors of a grid vertex
885 static const std::vector<ChVector2<int>> neighbors8{
886 ChVector2<int>(-1, -1), // SW
887 ChVector2<int>(0, -1), // S
888 ChVector2<int>(1, -1), // SE
889 ChVector2<int>(-1, 0), // W
890 ChVector2<int>(1, 0), // E
891 ChVector2<int>(-1, 1), // NW
892 ChVector2<int>(0, 1), // N
893 ChVector2<int>(1, 1) // NE
894 };
895
896 static const std::vector<ChVector2<int>> neighbors4{
897 ChVector2<int>(0, -1), // S
898 ChVector2<int>(-1, 0), // W
899 ChVector2<int>(1, 0), // E
900 ChVector2<int>(0, 1) // N
901 };
902
903 // Default implementation uses Map-Reduce for collecting ray intersection hits.
904 // The alternative is to simultaenously load the global map of hits while ray casting (using a critical section).
905 ////#define RAY_CASTING_WITH_CRITICAL_SECTION
906
907 // Reset the list of forces, and fills it with forces from a soil contact model.
ComputeInternalForces()908 void SCMDeformableSoil::ComputeInternalForces() {
909 // Initialize list of modified visualization mesh vertices (use any externally modified vertices)
910 std::vector<int> modified_vertices = m_external_modified_vertices;
911 m_external_modified_vertices.clear();
912
913 // Reset quantities at grid nodes modified over previous step
914 // (required for bulldozing effects and for proper visualization coloring)
915 for (const auto& ij : m_modified_nodes) {
916 auto& nr = m_grid_map.at(ij);
917 nr.sigma = 0;
918 nr.sinkage_elastic = 0;
919 nr.step_plastic_flow = 0;
920 nr.erosion = false;
921 nr.hit_level = 1e9;
922
923 // Update visualization (only color changes relevant here)
924 if (m_trimesh_shape && CheckMeshBounds(ij)) {
925 int iv = GetMeshVertexIndex(ij); // mesh vertex index
926 UpdateMeshVertexCoordinates(ij, iv, nr); // update vertex coordinates and color
927 modified_vertices.push_back(iv);
928 }
929 }
930
931 m_modified_nodes.clear();
932
933 // Reset timers
934 m_timer_moving_patches.reset();
935 m_timer_ray_testing.reset();
936 m_timer_ray_casting.reset();
937 m_timer_contact_patches.reset();
938 m_timer_contact_forces.reset();
939 m_timer_bulldozing.reset();
940 m_timer_bulldozing_boundary.reset();
941 m_timer_bulldozing_domain.reset();
942 m_timer_bulldozing_erosion.reset();
943 m_timer_visualization.reset();
944
945 // Reset the load list and map of contact forces
946 this->GetLoadList().clear();
947 m_contact_forces.clear();
948
949 // ---------------------
950 // Update moving patches
951 // ---------------------
952
953 m_timer_moving_patches.start();
954
955 // Update patch information (find range of grid indices)
956 if (m_moving_patch) {
957 for (auto& p : m_patches)
958 UpdateMovingPatch(p, m_Z);
959 } else {
960 assert(m_patches.size() == 1);
961 UpdateFixedPatch(m_patches[0]);
962 }
963
964 m_timer_moving_patches.stop();
965
966 // -------------------------
967 // Perform ray casting tests
968 // -------------------------
969
970 // Information of vertices with ray-cast hits
971 struct HitRecord {
972 ChContactable* contactable; // pointer to hit object
973 ChVector<> abs_point; // hit point, expressed in global frame
974 int patch_id; // index of associated patch id
975 };
976
977 // Hash-map for vertices with ray-cast hits
978 std::unordered_map<ChVector2<int>, HitRecord, CoordHash> hits;
979
980 m_num_ray_casts = 0;
981 m_num_ray_hits = 0;
982
983 m_timer_ray_casting.start();
984
985 #ifdef RAY_CASTING_WITH_CRITICAL_SECTION
986
987 int nthreads = GetSystem()->GetNumThreadsChrono();
988
989 // Loop through all moving patches (user-defined or default one)
990 for (auto& p : m_patches) {
991 // Loop through all vertices in the patch range
992 int num_ray_casts = 0;
993 #pragma omp parallel for num_threads(nthreads) reduction(+ : num_ray_casts)
994 for (int k = 0; k < p.m_range.size(); k++) {
995 ChVector2<int> ij = p.m_range[k];
996
997 // Move from (i, j) to (x, y, z) representation in the world frame
998 double x = ij.x() * m_delta;
999 double y = ij.y() * m_delta;
1000 double z;
1001 #pragma omp critical(SCM_ray_casting)
1002 z = GetHeight(ij);
1003
1004 ChVector<> vertex_abs = m_plane.TransformPointLocalToParent(ChVector<>(x, y, z));
1005
1006 // Create ray at current grid location
1007 collision::ChCollisionSystem::ChRayhitResult mrayhit_result;
1008 ChVector<> to = vertex_abs + m_Z * m_test_offset_up;
1009 ChVector<> from = to - m_Z * m_test_offset_down;
1010
1011 // Ray-OBB test (quick rejection)
1012 if (m_moving_patch && !RayOBBtest(p, from, m_Z))
1013 continue;
1014
1015 // Cast ray into collision system
1016 GetSystem()->GetCollisionSystem()->RayHit(from, to, mrayhit_result);
1017 num_ray_casts++;
1018
1019 if (mrayhit_result.hit) {
1020 #pragma omp critical(SCM_ray_casting)
1021 {
1022 // If this is the first hit from this node, initialize the node record
1023 if (m_grid_map.find(ij) == m_grid_map.end()) {
1024 m_grid_map.insert(std::make_pair(ij, NodeRecord(z, z, GetInitNormal(ij))));
1025 }
1026
1027 // Add to our map of hits to process
1028 HitRecord record = {mrayhit_result.hitModel->GetContactable(), mrayhit_result.abs_hitPoint, -1};
1029 hits.insert(std::make_pair(ij, record));
1030 m_num_ray_hits++;
1031 }
1032 }
1033 }
1034 m_num_ray_casts += num_ray_casts;
1035 }
1036
1037 #else
1038
1039 // Map-reduce approach (to eliminate critical section)
1040
1041 const int nthreads = GetSystem()->GetNumThreadsChrono();
1042 std::vector<std::unordered_map<ChVector2<int>, HitRecord, CoordHash>> t_hits(nthreads);
1043
1044 // Loop through all moving patches (user-defined or default one)
1045 for (auto& p : m_patches) {
1046 m_timer_ray_testing.start();
1047
1048 // Loop through all vertices in the patch range
1049 int num_ray_casts = 0;
1050 #pragma omp parallel for num_threads(nthreads) reduction(+:num_ray_casts)
1051 for (int k = 0; k < p.m_range.size(); k++) {
1052 int t_num = ChOMP::GetThreadNum();
1053 ChVector2<int> ij = p.m_range[k];
1054
1055 // Move from (i, j) to (x, y, z) representation in the world frame
1056 double x = ij.x() * m_delta;
1057 double y = ij.y() * m_delta;
1058 double z = GetHeight(ij);
1059
1060 ChVector<> vertex_abs = m_plane.TransformPointLocalToParent(ChVector<>(x, y, z));
1061
1062 // Create ray at current grid location
1063 collision::ChCollisionSystem::ChRayhitResult mrayhit_result;
1064 ChVector<> to = vertex_abs + m_Z * m_test_offset_up;
1065 ChVector<> from = to - m_Z * m_test_offset_down;
1066
1067 // Ray-OBB test (quick rejection)
1068 if (m_moving_patch && !RayOBBtest(p, from, m_Z))
1069 continue;
1070
1071 // Cast ray into collision system
1072 GetSystem()->GetCollisionSystem()->RayHit(from, to, mrayhit_result);
1073 num_ray_casts++;
1074
1075 if (mrayhit_result.hit) {
1076 // Add to our map of hits to process
1077 HitRecord record = {mrayhit_result.hitModel->GetContactable(), mrayhit_result.abs_hitPoint, -1};
1078 t_hits[t_num].insert(std::make_pair(ij, record));
1079 }
1080 }
1081
1082 m_timer_ray_testing.stop();
1083
1084 m_num_ray_casts += num_ray_casts;
1085
1086 // Sequential insertion in global hits
1087 for (int t_num = 0; t_num < nthreads; t_num++) {
1088
1089 for (auto& h : t_hits[t_num]) {
1090 // If this is the first hit from this node, initialize the node record
1091 if (m_grid_map.find(h.first) == m_grid_map.end()) {
1092 double z = GetInitHeight(h.first);
1093 m_grid_map.insert(std::make_pair(h.first, NodeRecord(z, z, GetInitNormal(h.first))));
1094 }
1095 ////hits.insert(h);
1096 }
1097
1098 hits.insert(t_hits[t_num].begin(), t_hits[t_num].end());
1099 t_hits[t_num].clear();
1100 }
1101 m_num_ray_hits = (int)hits.size();
1102 }
1103
1104 #endif
1105
1106 m_timer_ray_casting.stop();
1107
1108 // --------------------
1109 // Find contact patches
1110 // --------------------
1111
1112 m_timer_contact_patches.start();
1113
1114 // Collect hit vertices assigned to each contact patch.
1115 struct ContactPatchRecord {
1116 std::vector<ChVector2<>> points; // points in contact patch (in reference plane)
1117 std::vector<ChVector2<int>> nodes; // grid nodes in the contact patch
1118 double area; // contact patch area
1119 double perimeter; // contact patch perimeter
1120 double oob; // approximate value of 1/b
1121 };
1122 std::vector<ContactPatchRecord> contact_patches;
1123
1124 // Loop through all hit nodes and determine to which contact patch they belong.
1125 // Use a queue-based flood-filling algorithm based on the neighbors of each hit node.
1126 m_num_contact_patches = 0;
1127 for (auto& h : hits) {
1128 if (h.second.patch_id != -1)
1129 continue;
1130
1131 ChVector2<int> ij = h.first;
1132
1133 // Make a new contact patch and add this hit node to it
1134 h.second.patch_id = m_num_contact_patches++;
1135 ContactPatchRecord patch;
1136 patch.nodes.push_back(ij);
1137 patch.points.push_back(ChVector2<>(m_delta * ij.x(), m_delta * ij.y()));
1138
1139 // Add current node to the work queue
1140 std::queue<ChVector2<int>> todo;
1141 todo.push(ij);
1142
1143 while (!todo.empty()) {
1144 auto crt = hits.find(todo.front()); // Current hit node is first element in queue
1145 todo.pop(); // Remove first element from queue
1146
1147 ChVector2<int> crt_ij = crt->first;
1148 int crt_patch = crt->second.patch_id;
1149
1150 // Loop through the neighbors of the current hit node
1151 for (int k = 0; k < 4; k++) {
1152 ChVector2<int> nbr_ij = crt_ij + neighbors4[k];
1153 // If neighbor is not a hit node, move on
1154 auto nbr = hits.find(nbr_ij);
1155 if (nbr == hits.end())
1156 continue;
1157 // If neighbor already assigned to a contact patch, move on
1158 if (nbr->second.patch_id != -1)
1159 continue;
1160 // Assign neighbor to the same contact patch
1161 nbr->second.patch_id = crt_patch;
1162 // Add neighbor point to patch lists
1163 patch.nodes.push_back(nbr_ij);
1164 patch.points.push_back(ChVector2<>(m_delta * nbr_ij.x(), m_delta * nbr_ij.y()));
1165 // Add neighbor to end of work queue
1166 todo.push(nbr_ij);
1167 }
1168 }
1169 contact_patches.push_back(patch);
1170 }
1171
1172 // Calculate area and perimeter of each contact patch.
1173 // Calculate approximation to Beker term 1/b.
1174 for (auto& p : contact_patches) {
1175 utils::ChConvexHull2D ch(p.points);
1176 p.area = ch.GetArea();
1177 p.perimeter = ch.GetPerimeter();
1178 if (p.area < 1e-6) {
1179 p.oob = 0;
1180 } else {
1181 p.oob = p.perimeter / (2 * p.area);
1182 }
1183 }
1184
1185 m_timer_contact_patches.stop();
1186
1187 // ----------------------
1188 // Compute contact forces
1189 // ----------------------
1190
1191 m_timer_contact_forces.start();
1192
1193 // Initialize local values for the soil parameters
1194 double Bekker_Kphi = m_Bekker_Kphi;
1195 double Bekker_Kc = m_Bekker_Kc;
1196 double Bekker_n = m_Bekker_n;
1197 double Mohr_cohesion = m_Mohr_cohesion;
1198 double Mohr_mu = m_Mohr_mu;
1199 double Janosi_shear = m_Janosi_shear;
1200 double elastic_K = m_elastic_K;
1201 double damping_R = m_damping_R;
1202
1203 // Process only hit nodes
1204 for (auto& h : hits) {
1205 ChVector2<> ij = h.first;
1206
1207 auto& nr = m_grid_map.at(ij); // node record
1208 const double& ca = nr.normal.z(); // cosine of angle between local normal and SCM plane vertical
1209
1210 ChContactable* contactable = h.second.contactable;
1211 const ChVector<>& hit_point_abs = h.second.abs_point;
1212 int patch_id = h.second.patch_id;
1213
1214 auto hit_point_loc = m_plane.TransformPointParentToLocal(hit_point_abs);
1215
1216 if (m_soil_fun) {
1217 double Mohr_friction;
1218 m_soil_fun->Set(hit_point_loc, Bekker_Kphi, Bekker_Kc, Bekker_n, Mohr_cohesion, Mohr_friction, Janosi_shear,
1219 elastic_K, damping_R);
1220 Mohr_mu = std::tan(Mohr_friction * CH_C_DEG_TO_RAD);
1221 }
1222
1223 nr.hit_level = hit_point_loc.z(); // along SCM z axis
1224 double p_hit_offset = ca * (nr.level_initial - nr.hit_level); // along local normal direction
1225
1226 // Elastic try (along local normal direction)
1227 nr.sigma = elastic_K * (p_hit_offset - nr.sinkage_plastic);
1228
1229 // Handle unilaterality
1230 if (nr.sigma < 0) {
1231 nr.sigma = 0;
1232 continue;
1233 }
1234
1235 // Mark current node as modified
1236 m_modified_nodes.push_back(ij);
1237
1238 // Calculate velocity at touched grid node
1239 ChVector<> point_local(ij.x() * m_delta, ij.y() * m_delta, nr.level);
1240 ChVector<> point_abs = m_plane.TransformPointLocalToParent(point_local);
1241 ChVector<> speed_abs = contactable->GetContactPointSpeed(point_abs);
1242
1243 // Calculate normal and tangent directions (expressed in absolute frame)
1244 ChVector<> N = m_plane.TransformDirectionLocalToParent(nr.normal);
1245 double Vn = Vdot(speed_abs, N);
1246 ChVector<> T = -(speed_abs - Vn * N);
1247 T.Normalize();
1248
1249 // Update total sinkage and current level for this hit node
1250 nr.sinkage = p_hit_offset;
1251 nr.level = nr.hit_level;
1252
1253 // Accumulate shear for Janosi-Hanamoto (along local tangent direction)
1254 nr.kshear += Vdot(speed_abs, -T) * GetSystem()->GetStep();
1255
1256 // Plastic correction (along local normal direction)
1257 if (nr.sigma > nr.sigma_yield) {
1258 // Bekker formula
1259 nr.sigma = (contact_patches[patch_id].oob * Bekker_Kc + Bekker_Kphi) * pow(nr.sinkage, Bekker_n);
1260 nr.sigma_yield = nr.sigma;
1261 double old_sinkage_plastic = nr.sinkage_plastic;
1262 nr.sinkage_plastic = nr.sinkage - nr.sigma / elastic_K;
1263 nr.step_plastic_flow = (nr.sinkage_plastic - old_sinkage_plastic) / GetSystem()->GetStep();
1264 }
1265
1266 // Elastic sinkage (along local normal direction)
1267 nr.sinkage_elastic = nr.sinkage - nr.sinkage_plastic;
1268
1269 // Add compressive speed-proportional damping (not clamped by pressure yield)
1270 ////if (Vn < 0) {
1271 nr.sigma += -Vn * damping_R;
1272 ////}
1273
1274 // Mohr-Coulomb
1275 double tau_max = Mohr_cohesion + nr.sigma * Mohr_mu;
1276
1277 // Janosi-Hanamoto (along local tangent direction)
1278 nr.tau = tau_max * (1.0 - exp(-(nr.kshear / Janosi_shear)));
1279
1280 // Calculate normal and tangential forces (in local node directions).
1281 // If specified, combine properties for soil-contactable interaction and soil-soil interaction.
1282 ChVector<> Fn = N * m_area * nr.sigma;
1283 ChVector<> Ft;
1284
1285 //// TODO: take into account "tread height" (add to SCMContactableData)?
1286
1287 if (auto cprops = contactable->GetUserData<vehicle::SCMContactableData>()) {
1288 // Use weighted sum of soil-contactable and soil-soil parameters
1289 double c_tau_max = cprops->Mohr_cohesion + nr.sigma * cprops->Mohr_mu;
1290 double c_tau = c_tau_max * (1.0 - exp(-(nr.kshear / cprops->Janosi_shear)));
1291 double ratio = cprops->area_ratio;
1292 Ft = T * m_area * ((1 - ratio) * nr.tau + ratio * c_tau);
1293 } else {
1294 // Use only soil-soil parameters
1295 Ft = T * m_area * nr.tau;
1296 }
1297
1298 if (ChBody* rigidbody = dynamic_cast<ChBody*>(contactable)) {
1299 // [](){} Trick: no deletion for this shared ptr, since 'rigidbody' was not a new ChBody()
1300 // object, but an already used pointer because mrayhit_result.hitModel->GetPhysicsItem()
1301 // cannot return it as shared_ptr, as needed by the ChLoadBodyForce:
1302 std::shared_ptr<ChBody> srigidbody(rigidbody, [](ChBody*) {});
1303 std::shared_ptr<ChLoadBodyForce> mload(new ChLoadBodyForce(srigidbody, Fn + Ft, false, point_abs, false));
1304 this->Add(mload);
1305
1306 // Accumulate contact force for this rigid body.
1307 // The resultant force is assumed to be applied at the body COM.
1308 // All components of the generalized terrain force are expressed in the global frame.
1309 auto itr = m_contact_forces.find(contactable);
1310 if (itr == m_contact_forces.end()) {
1311 // Create new entry and initialize generalized force.
1312 ChVector<> force = Fn + Ft;
1313 TerrainForce frc;
1314 frc.point = srigidbody->GetPos();
1315 frc.force = force;
1316 frc.moment = Vcross(Vsub(point_abs, srigidbody->GetPos()), force);
1317 m_contact_forces.insert(std::make_pair(contactable, frc));
1318 } else {
1319 // Update generalized force.
1320 ChVector<> force = Fn + Ft;
1321 itr->second.force += force;
1322 itr->second.moment += Vcross(Vsub(point_abs, srigidbody->GetPos()), force);
1323 }
1324 } else if (ChLoadableUV* surf = dynamic_cast<ChLoadableUV*>(contactable)) {
1325 // [](){} Trick: no deletion for this shared ptr
1326 std::shared_ptr<ChLoadableUV> ssurf(surf, [](ChLoadableUV*) {});
1327 std::shared_ptr<ChLoad<ChLoaderForceOnSurface>> mload(new ChLoad<ChLoaderForceOnSurface>(ssurf));
1328 mload->loader.SetForce(Fn + Ft);
1329 mload->loader.SetApplication(0.5, 0.5); //***TODO*** set UV, now just in middle
1330 this->Add(mload);
1331
1332 // Accumulate contact forces for this surface.
1333 //// TODO
1334 }
1335
1336 // Update grid node height (in local SCM frame, along SCM z axis)
1337 nr.level = nr.level_initial - nr.sinkage / ca;
1338
1339 } // end loop on ray hits
1340
1341 m_timer_contact_forces.stop();
1342
1343 // --------------------------------------------------
1344 // Flow material to the side of rut, using heuristics
1345 // --------------------------------------------------
1346
1347 m_timer_bulldozing.start();
1348
1349 m_num_erosion_nodes = 0;
1350
1351 if (m_bulldozing) {
1352 typedef std::unordered_set<ChVector2<int>, CoordHash> NodeSet;
1353
1354 // Maximum level change between neighboring nodes (smoothing phase)
1355 double dy_lim = m_delta * m_erosion_slope;
1356
1357 // (1) Raise boundaries of each contact patch
1358 m_timer_bulldozing_boundary.start();
1359
1360 NodeSet boundary; // union of contact patch boundaries
1361 for (auto p : contact_patches) {
1362 NodeSet p_boundary; // boundary of effective contact patch
1363
1364 // Calculate the displaced material from all touched nodes and identify boundary
1365 double tot_step_flow = 0;
1366 for (const auto& ij : p.nodes) { // for each node in contact patch
1367 const auto& nr = m_grid_map.at(ij); // get node record
1368 if (nr.sigma <= 0) // if node not touched
1369 continue; // skip (not in effective patch)
1370 tot_step_flow += nr.step_plastic_flow; // accumulate displaced material
1371 for (int k = 0; k < 4; k++) { // check each node neighbor
1372 ChVector2<int> nbr_ij = ij + neighbors4[k]; // neighbor node coordinates
1373 ////if (!CheckMeshBounds(nbr_ij)) // if neighbor out of bounds
1374 //// continue; // skip neighbor
1375 if (m_grid_map.find(nbr_ij) == m_grid_map.end()) // if neighbor not yet recorded
1376 p_boundary.insert(nbr_ij); // set neighbor as boundary
1377 else if (m_grid_map.at(nbr_ij).sigma <= 0) // if neighbor not touched
1378 p_boundary.insert(nbr_ij); // set neighbor as boundary
1379 }
1380 }
1381 tot_step_flow *= GetSystem()->GetStep();
1382
1383 // Target raise amount for each boundary node (unless clamped)
1384 double diff = m_flow_factor * tot_step_flow / p_boundary.size();
1385
1386 // Raise boundary (create a sharp spike which will be later smoothed out with erosion)
1387 for (const auto& ij : p_boundary) { // for each node in bndry
1388 m_modified_nodes.push_back(ij); // mark as modified
1389 if (m_grid_map.find(ij) == m_grid_map.end()) { // if not yet recorded
1390 double z = GetInitHeight(ij); // undeformed height
1391 const ChVector<>& n = GetInitNormal(ij); // terrain normal
1392 m_grid_map.insert(std::make_pair(ij, NodeRecord(z, z, n))); // add new node record
1393 m_modified_nodes.push_back(ij); // mark as modified
1394 } //
1395 auto& nr = m_grid_map.at(ij); // node record
1396 nr.erosion = true; // add to erosion domain
1397 AddMaterialToNode(diff, nr); // add raise amount
1398 }
1399
1400 // Accumulate boundary
1401 boundary.insert(p_boundary.begin(), p_boundary.end());
1402
1403 } // end for contact_patches
1404
1405 m_timer_bulldozing_boundary.stop();
1406
1407 // (2) Calculate erosion domain (dilate boundary)
1408 m_timer_bulldozing_domain.start();
1409
1410 NodeSet erosion_domain = boundary;
1411 NodeSet erosion_front = boundary; // initialize erosion front to boundary nodes
1412 for (int i = 0; i < m_erosion_propagations; i++) {
1413 NodeSet front; // new erosion front
1414 for (const auto& ij : erosion_front) { // for each node in current erosion front
1415 for (int k = 0; k < 4; k++) { // check each of its neighbors
1416 ChVector2<int> nbr_ij = ij + neighbors4[k]; // neighbor node coordinates
1417 ////if (!CheckMeshBounds(nbr_ij)) // if out of bounds
1418 //// continue; // ignore neighbor
1419 if (m_grid_map.find(nbr_ij) == m_grid_map.end()) { // if neighbor not yet recorded
1420 double z = GetInitHeight(nbr_ij); // undeformed height at neighbor location
1421 const ChVector<>& n = GetInitNormal(nbr_ij); // terrain normal at neighbor location
1422 NodeRecord nr(z, z, n); // create new record
1423 nr.erosion = true; // include in erosion domain
1424 m_grid_map.insert(std::make_pair(nbr_ij, nr)); // add new node record
1425 front.insert(nbr_ij); // add neighbor to new front
1426 m_modified_nodes.push_back(nbr_ij); // mark as modified
1427 } else { // if neighbor previously recorded
1428 NodeRecord& nr = m_grid_map.at(nbr_ij); // get existing record
1429 if (!nr.erosion && nr.sigma <= 0) { // if neighbor not touched
1430 nr.erosion = true; // include in erosion domain
1431 front.insert(nbr_ij); // add neighbor to new front
1432 m_modified_nodes.push_back(nbr_ij); // mark as modified
1433 }
1434 }
1435 }
1436 }
1437 erosion_domain.insert(front.begin(), front.end()); // add current front to erosion domain
1438 erosion_front = front; // advance erosion front
1439 }
1440
1441 m_num_erosion_nodes = static_cast<int>(erosion_domain.size());
1442 m_timer_bulldozing_domain.stop();
1443
1444 // (3) Erosion algorithm on domain
1445 m_timer_bulldozing_erosion.start();
1446
1447 for (int iter = 0; iter < m_erosion_iterations; iter++) {
1448 for (const auto& ij : erosion_domain) {
1449 auto& nr = m_grid_map.at(ij);
1450 for (int k = 0; k < 4; k++) {
1451 ChVector2<int> nbr_ij = ij + neighbors4[k];
1452 auto rec = m_grid_map.find(nbr_ij);
1453 if (rec == m_grid_map.end())
1454 continue;
1455 auto& nbr_nr = rec->second;
1456
1457 // (3.1) Flow remaining material to neighbor
1458 double diff = 0.5 * (nr.massremainder - nbr_nr.massremainder) / 4; //// TODO: rethink this!
1459 if (diff > 0) {
1460 RemoveMaterialFromNode(diff, nr);
1461 AddMaterialToNode(diff, nbr_nr);
1462 }
1463
1464 // (3.2) Smoothing
1465 if (nbr_nr.sigma == 0) {
1466 double dy = (nr.level + nr.massremainder) - (nbr_nr.level + nbr_nr.massremainder);
1467 diff = 0.5 * (std::abs(dy) - dy_lim) / 4; //// TODO: rethink this!
1468 if (diff > 0) {
1469 if (dy > 0) {
1470 RemoveMaterialFromNode(diff, nr);
1471 AddMaterialToNode(diff, nbr_nr);
1472 } else {
1473 RemoveMaterialFromNode(diff, nbr_nr);
1474 AddMaterialToNode(diff, nr);
1475 }
1476 }
1477 }
1478 }
1479 }
1480 }
1481
1482 m_timer_bulldozing_erosion.stop();
1483
1484 } // end do_bulldozing
1485
1486 m_timer_bulldozing.stop();
1487
1488 // --------------------
1489 // Update visualization
1490 // --------------------
1491
1492 m_timer_visualization.start();
1493
1494 if (m_trimesh_shape) {
1495 // Loop over list of modified nodes and adjust corresponding mesh vertices.
1496 // If not rendering a wireframe mesh, also update normals.
1497 for (const auto& ij : m_modified_nodes) {
1498 if (!CheckMeshBounds(ij)) // if node outside mesh
1499 continue; // do nothing
1500 const auto& nr = m_grid_map.at(ij); // grid node record
1501 int iv = GetMeshVertexIndex(ij); // mesh vertex index
1502 UpdateMeshVertexCoordinates(ij, iv, nr); // update vertex coordinates and color
1503 modified_vertices.push_back(iv); // cache in list of modified mesh vertices
1504 if (!m_trimesh_shape->IsWireframe()) // if not wireframe
1505 UpdateMeshVertexNormal(ij, iv); // update vertex normal
1506 }
1507
1508 m_trimesh_shape->SetModifiedVertices(modified_vertices);
1509 }
1510
1511 m_timer_visualization.stop();
1512 }
1513
AddMaterialToNode(double amount,NodeRecord & nr)1514 void SCMDeformableSoil::AddMaterialToNode(double amount, NodeRecord& nr) {
1515 if (amount > nr.hit_level - nr.level) { // if not possible to assign all mass
1516 nr.massremainder += amount - (nr.hit_level - nr.level); // material to be further propagated
1517 amount = nr.hit_level - nr.level; // clamp raise amount
1518 } //
1519 nr.level += amount; // modify node level
1520 nr.level_initial += amount; // reset node initial level
1521 }
1522
RemoveMaterialFromNode(double amount,NodeRecord & nr)1523 void SCMDeformableSoil::RemoveMaterialFromNode(double amount, NodeRecord& nr) {
1524 if (nr.massremainder > amount) { // if too much remainder material
1525 nr.massremainder -= amount; // decrease remainder material
1526 /*amount = 0;*/ // ???
1527 } else if (nr.massremainder < amount && nr.massremainder > 0) { // if not enough remainder material
1528 amount -= nr.massremainder; // clamp removed amount
1529 nr.massremainder = 0; // remainder material exhausted
1530 } //
1531 nr.level -= amount; // modify node level
1532 nr.level_initial -= amount; // reset node initial level
1533 }
1534
1535 // Update vertex position and color in visualization mesh
UpdateMeshVertexCoordinates(const ChVector2<int> ij,int iv,const NodeRecord & nr)1536 void SCMDeformableSoil::UpdateMeshVertexCoordinates(const ChVector2<int> ij, int iv, const NodeRecord& nr) {
1537 auto& trimesh = *m_trimesh_shape->GetMesh();
1538 std::vector<ChVector<>>& vertices = trimesh.getCoordsVertices();
1539 std::vector<ChVector<float>>& colors = trimesh.getCoordsColors();
1540
1541 // Update visualization mesh vertex position
1542 vertices[iv] = m_plane.TransformPointLocalToParent(ChVector<>(ij.x() * m_delta, ij.y() * m_delta, nr.level));
1543
1544 // Update visualization mesh vertex color
1545 if (m_plot_type != SCMDeformableTerrain::PLOT_NONE) {
1546 ChColor mcolor;
1547 switch (m_plot_type) {
1548 case SCMDeformableTerrain::PLOT_LEVEL:
1549 mcolor = ChColor::ComputeFalseColor(nr.level, m_plot_v_min, m_plot_v_max);
1550 break;
1551 case SCMDeformableTerrain::PLOT_LEVEL_INITIAL:
1552 mcolor = ChColor::ComputeFalseColor(nr.level_initial, m_plot_v_min, m_plot_v_max);
1553 break;
1554 case SCMDeformableTerrain::PLOT_SINKAGE:
1555 mcolor = ChColor::ComputeFalseColor(nr.sinkage, m_plot_v_min, m_plot_v_max);
1556 break;
1557 case SCMDeformableTerrain::PLOT_SINKAGE_ELASTIC:
1558 mcolor = ChColor::ComputeFalseColor(nr.sinkage_elastic, m_plot_v_min, m_plot_v_max);
1559 break;
1560 case SCMDeformableTerrain::PLOT_SINKAGE_PLASTIC:
1561 mcolor = ChColor::ComputeFalseColor(nr.sinkage_plastic, m_plot_v_min, m_plot_v_max);
1562 break;
1563 case SCMDeformableTerrain::PLOT_STEP_PLASTIC_FLOW:
1564 mcolor = ChColor::ComputeFalseColor(nr.step_plastic_flow, m_plot_v_min, m_plot_v_max);
1565 break;
1566 case SCMDeformableTerrain::PLOT_K_JANOSI:
1567 mcolor = ChColor::ComputeFalseColor(nr.kshear, m_plot_v_min, m_plot_v_max);
1568 break;
1569 case SCMDeformableTerrain::PLOT_PRESSURE:
1570 mcolor = ChColor::ComputeFalseColor(nr.sigma, m_plot_v_min, m_plot_v_max);
1571 break;
1572 case SCMDeformableTerrain::PLOT_PRESSURE_YELD:
1573 mcolor = ChColor::ComputeFalseColor(nr.sigma_yield, m_plot_v_min, m_plot_v_max);
1574 break;
1575 case SCMDeformableTerrain::PLOT_SHEAR:
1576 mcolor = ChColor::ComputeFalseColor(nr.tau, m_plot_v_min, m_plot_v_max);
1577 break;
1578 case SCMDeformableTerrain::PLOT_MASSREMAINDER:
1579 mcolor = ChColor::ComputeFalseColor(nr.massremainder, m_plot_v_min, m_plot_v_max);
1580 break;
1581 case SCMDeformableTerrain::PLOT_ISLAND_ID:
1582 if (nr.erosion)
1583 mcolor = ChColor(0, 0, 0);
1584 if (nr.sigma > 0)
1585 mcolor = ChColor(1, 0, 0);
1586 break;
1587 case SCMDeformableTerrain::PLOT_IS_TOUCHED:
1588 if (nr.sigma > 0)
1589 mcolor = ChColor(1, 0, 0);
1590 else
1591 mcolor = ChColor(0, 0, 1);
1592 break;
1593 case SCMDeformableTerrain::PLOT_NONE:
1594 break;
1595 }
1596 colors[iv] = {mcolor.R, mcolor.G, mcolor.B};
1597 }
1598 }
1599
1600 // Update vertex normal in visualization mesh.
UpdateMeshVertexNormal(const ChVector2<int> ij,int iv)1601 void SCMDeformableSoil::UpdateMeshVertexNormal(const ChVector2<int> ij, int iv) {
1602 auto& trimesh = *m_trimesh_shape->GetMesh();
1603 std::vector<ChVector<>>& vertices = trimesh.getCoordsVertices();
1604 std::vector<ChVector<>>& normals = trimesh.getCoordsNormals();
1605 std::vector<ChVector<int>>& idx_normals = trimesh.getIndicesNormals();
1606
1607 // Average normals from adjacent faces
1608 normals[iv] = ChVector<>(0, 0, 0);
1609 auto faces = GetMeshFaceIndices(ij);
1610 for (auto f : faces) {
1611 ChVector<> nrm = Vcross(vertices[idx_normals[f][1]] - vertices[idx_normals[f][0]],
1612 vertices[idx_normals[f][2]] - vertices[idx_normals[f][0]]);
1613 nrm.Normalize();
1614 normals[iv] += nrm;
1615 }
1616 normals[iv] /= (double)faces.size();
1617 }
1618
1619 // Get the heights of modified grid nodes.
GetModifiedNodes(bool all_nodes) const1620 std::vector<SCMDeformableTerrain::NodeLevel> SCMDeformableSoil::GetModifiedNodes(bool all_nodes) const {
1621 std::vector<SCMDeformableTerrain::NodeLevel> nodes;
1622 if (all_nodes) {
1623 for (const auto& nr : m_grid_map) {
1624 nodes.push_back(std::make_pair(nr.first, nr.second.level));
1625 }
1626 } else {
1627 for (const auto& ij : m_modified_nodes) {
1628 auto rec = m_grid_map.find(ij);
1629 assert(rec != m_grid_map.end());
1630 nodes.push_back(std::make_pair(ij, rec->second.level));
1631 }
1632 }
1633 return nodes;
1634 }
1635
1636 // Modify the level of grid nodes from the given list.
1637 // NOTE: We set only the level of the specified nodes and none of the other soil properties.
1638 // As such, some plot types may be incorrect at these nodes.
SetModifiedNodes(const std::vector<SCMDeformableTerrain::NodeLevel> & nodes)1639 void SCMDeformableSoil::SetModifiedNodes(const std::vector<SCMDeformableTerrain::NodeLevel>& nodes) {
1640 for (const auto& n : nodes) {
1641 // Modify existing entry in grid map or insert new one
1642 m_grid_map[n.first] = SCMDeformableSoil::NodeRecord(n.second, n.second, GetInitNormal(n.first));
1643 }
1644
1645 // Update visualization
1646 if (m_trimesh_shape) {
1647 for (const auto& n : nodes) {
1648 auto ij = n.first; // grid location
1649 if (!CheckMeshBounds(ij)) // if outside mesh
1650 continue; // do nothing
1651 const auto& nr = m_grid_map.at(ij); // grid node record
1652 int iv = GetMeshVertexIndex(ij); // mesh vertex index
1653 UpdateMeshVertexCoordinates(ij, iv, nr); // update vertex coordinates and color
1654 if (!m_trimesh_shape->IsWireframe()) // if not in wireframe mode
1655 UpdateMeshVertexNormal(ij, iv); // update vertex normal
1656 m_external_modified_vertices.push_back(iv); // cache in list
1657 }
1658 }
1659 }
1660
1661 } // end namespace vehicle
1662 } // end namespace chrono
1663