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: Radu Serban
13 // =============================================================================
14 //
15 // Chrono demonstration of using contact callbacks for smooth contacts
16 // (penalty-based) in Chrono::Multicore.
17 //
18 // The global reference frame has Y up.
19 //
20 // =============================================================================
21
22 #include <cstdio>
23 #include <cmath>
24
25 #include "chrono/utils/ChUtilsCreators.h"
26 #include "chrono_multicore/physics/ChSystemMulticore.h"
27
28 #include "chrono_opengl/ChOpenGLWindow.h"
29
30 using namespace chrono;
31
32 // -----------------------------------------------------------------------------
33 // Callback class for contact reporting
34 // -----------------------------------------------------------------------------
35 class ContactReporter : public ChContactContainer::ReportContactCallback {
36 public:
ContactReporter(std::shared_ptr<ChBody> obj1,std::shared_ptr<ChBody> obj2)37 ContactReporter(std::shared_ptr<ChBody> obj1, std::shared_ptr<ChBody> obj2) : m_obj1(obj1), m_obj2(obj2) {}
38
39 private:
OnReportContact(const ChVector<> & pA,const ChVector<> & pB,const ChMatrix33<> & plane_coord,const double & distance,const double & eff_radius,const ChVector<> & cforce,const ChVector<> & ctorque,ChContactable * modA,ChContactable * modB)40 virtual bool OnReportContact(const ChVector<>& pA,
41 const ChVector<>& pB,
42 const ChMatrix33<>& plane_coord,
43 const double& distance,
44 const double& eff_radius,
45 const ChVector<>& cforce,
46 const ChVector<>& ctorque,
47 ChContactable* modA,
48 ChContactable* modB) override {
49 // Check if contact involves obj1
50 if (modA == m_obj1.get()) {
51 printf(" A contact on Obj 1 at pos: %7.3f %7.3f %7.3f", pA.x(), pA.y(), pA.z());
52 } else if (modB == m_obj1.get()) {
53 printf(" B contact on Obj 1 at pos: %7.3f %7.3f %7.3f", pB.x(), pB.y(), pB.z());
54 }
55
56 // Check if contact involves obj2
57 if (modA == m_obj2.get()) {
58 printf(" A contact on Obj 2 at pos: %7.3f %7.3f %7.3f", pA.x(), pA.y(), pA.z());
59 } else if (modB == m_obj2.get()) {
60 printf(" B contact on Obj 2 at pos: %7.3f %7.3f %7.3f", pB.x(), pB.y(), pB.z());
61 }
62
63 const ChVector<>& nrm = plane_coord.Get_A_Xaxis();
64 printf(" nrm: %7.3f, %7.3f %7.3f", nrm.x(), nrm.y(), nrm.z());
65 printf(" frc: %7.3f %7.3f %7.3f", cforce.x(), cforce.y(), cforce.z());
66 printf(" trq: %7.3f, %7.3f %7.3f", ctorque.x(), ctorque.y(), ctorque.z());
67 printf(" penetration: %8.4f eff. radius: %7.3f\n", distance, eff_radius);
68
69 return true;
70 }
71
72 std::shared_ptr<ChBody> m_obj1;
73 std::shared_ptr<ChBody> m_obj2;
74 };
75
76 // -----------------------------------------------------------------------------
77 // Callback class for modifying composite material
78 // -----------------------------------------------------------------------------
79 class ContactMaterial : public ChContactContainer::AddContactCallback {
80 public:
OnAddContact(const collision::ChCollisionInfo & contactinfo,ChMaterialComposite * const material)81 virtual void OnAddContact(const collision::ChCollisionInfo& contactinfo,
82 ChMaterialComposite* const material) override {
83 // Downcast to appropriate composite material type
84 auto mat = static_cast<ChMaterialCompositeSMC* const>(material);
85
86 // Set different friction for left/right halfs
87 float friction = (contactinfo.vpA.z() > 0) ? 0.3f : 0.8f;
88 mat->mu_eff = friction;
89 }
90 };
91
main(int argc,char * argv[])92 int main(int argc, char* argv[]) {
93 std::cout << "Copyright (c) 2020 projectchrono.org\nChrono version: " << CHRONO_VERSION << std::endl;
94
95 // Parameters
96 float friction = 0.6f;
97
98 // Create the system
99 ChSystemMulticoreSMC system;
100 system.Set_G_acc(ChVector<>(0, -10, 0));
101
102 // Create a contact material, shared among all bodies
103 auto material = chrono_types::make_shared<ChMaterialSurfaceSMC>();
104 material->SetFriction(friction);
105
106 // Add bodies
107 auto container = std::shared_ptr<ChBody>(system.NewBody());
108 system.Add(container);
109 container->SetPos(ChVector<>(0, 0, 0));
110 container->SetBodyFixed(true);
111 container->SetIdentifier(-1);
112
113 container->SetCollide(true);
114 container->GetCollisionModel()->ClearModel();
115 utils::AddBoxGeometry(container.get(), material, ChVector<>(4, 0.5, 4), ChVector<>(0, -0.5, 0));
116 container->GetCollisionModel()->BuildModel();
117
118 auto obj1 = std::shared_ptr<ChBody>(system.NewBody());
119 obj1->SetMass(10);
120 obj1->SetInertiaXX(ChVector<>(1, 1, 1));
121 obj1->SetPos(ChVector<>(-1, 0.21, -1));
122 obj1->SetPos_dt(ChVector<>(5, 0, 0));
123
124 obj1->SetCollide(true);
125 obj1->GetCollisionModel()->ClearModel();
126 utils::AddCapsuleGeometry(obj1.get(), material, 0.2, 0.4, ChVector<>(0), Q_from_AngZ(CH_C_PI_2));
127 obj1->GetCollisionModel()->BuildModel();
128
129 system.AddBody(obj1);
130
131 auto obj2 = std::shared_ptr<ChBody>(system.NewBody());
132 obj2->SetMass(10);
133 obj2->SetInertiaXX(ChVector<>(1, 1, 1));
134 obj2->SetPos(ChVector<>(-1, 0.21, +1));
135 obj2->SetPos_dt(ChVector<>(5, 0, 0));
136
137 obj2->SetCollide(true);
138 obj2->GetCollisionModel()->ClearModel();
139 utils::AddCapsuleGeometry(obj2.get(), material, 0.2, 0.4, ChVector<>(0), Q_from_AngZ(CH_C_PI_2));
140 obj2->GetCollisionModel()->BuildModel();
141
142 system.AddBody(obj2);
143
144 // Create the visualization window
145 opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
146 gl_window.Initialize(1280, 720, "SMC callbacks", &system);
147 gl_window.SetCamera(ChVector<>(4, 4, -5), ChVector<>(0, 0, 0), ChVector<>(0, 1, 0));
148 gl_window.SetRenderMode(opengl::WIREFRAME);
149
150 // Callback for contact reporting
151 auto creporter = chrono_types::make_shared<ContactReporter>(obj1, obj2);
152
153 // Callback for contact addition
154 auto cmaterial = chrono_types::make_shared<ContactMaterial>();
155 system.GetContactContainer()->RegisterAddContactCallback(cmaterial);
156
157 // Simulate system
158 while (gl_window.Active()) {
159 gl_window.DoStepDynamics(1e-3);
160 gl_window.Render();
161
162 // Process contacts
163 std::cout << system.GetChTime() << " " << system.GetNcontacts() << std::endl;
164 system.GetContactContainer()->ReportAllContacts(creporter);
165
166 // Cumulative contact force and torque on objects (as applied to COM)
167 ChVector<> frc1 = obj1->GetContactForce();
168 ChVector<> trq1 = obj1->GetContactTorque();
169 printf(" Obj 1 contact force at COM: %7.3f %7.3f %7.3f", frc1.x(), frc1.y(), frc1.z());
170 printf(" contact torque at COM: %7.3f %7.3f %7.3f\n", trq1.x(), trq1.y(), trq1.z());
171 ChVector<> frc2 = obj2->GetContactForce();
172 ChVector<> trq2 = obj2->GetContactTorque();
173 printf(" Obj 2 contact force at COM: %7.3f %7.3f %7.3f", frc2.x(), frc2.y(), frc2.z());
174 printf(" contact torque at COM: %7.3f %7.3f %7.3f\n", trq2.x(), trq2.y(), trq2.z());
175 }
176
177 return 0;
178 }
179