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 non-smooth contacts
16 // (complementarity-based).
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/physics/ChSystemNSC.h"
27
28 #include "chrono_irrlicht/ChIrrApp.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> box1,std::shared_ptr<ChBody> box2)37 ContactReporter(std::shared_ptr<ChBody> box1, std::shared_ptr<ChBody> box2) : m_box1(box1), m_box2(box2) {}
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
50
51
52 // Check if contact involves box1
53 if (modA == m_box1.get()) {
54 printf(" A contact on Box 1 at pos: %7.3f %7.3f %7.3f", pA.x(), pA.y(), pA.z());
55 } else if (modB == m_box1.get()) {
56 printf(" B contact on Box 1 at pos: %7.3f %7.3f %7.3f", pB.x(), pB.y(), pB.z());
57 }
58
59 // Check if contact involves box2
60 if (modA == m_box2.get()) {
61 printf(" A contact on Box 2 at pos: %7.3f %7.3f %7.3f", pA.x(), pA.y(), pA.z());
62 } else if (modB == m_box2.get()) {
63 printf(" B contact on Box 2 at pos: %7.3f %7.3f %7.3f", pB.x(), pB.y(), pB.z());
64 }
65
66 const ChVector<>& nrm = plane_coord.Get_A_Xaxis();
67 printf(" nrm: %7.3f, %7.3f %7.3f", nrm.x(), nrm.y(), nrm.z());
68 printf(" frc: %7.3f %7.3f %7.3f", cforce.x(), cforce.y(), cforce.z());
69 printf(" trq: %7.3f, %7.3f %7.3f", ctorque.x(), ctorque.y(), ctorque.z());
70 printf(" penetration: %8.4f eff. radius: %7.3f\n", distance, eff_radius);
71
72 return true;
73 }
74
75 std::shared_ptr<ChBody> m_box1;
76 std::shared_ptr<ChBody> m_box2;
77 };
78
79 // -----------------------------------------------------------------------------
80 // Callback class for modifying composite material
81 // -----------------------------------------------------------------------------
82 class ContactMaterial : public ChContactContainer::AddContactCallback {
83 public:
OnAddContact(const collision::ChCollisionInfo & contactinfo,ChMaterialComposite * const material)84 virtual void OnAddContact(const collision::ChCollisionInfo& contactinfo,
85 ChMaterialComposite* const material) override {
86 // Downcast to appropriate composite material type
87 auto mat = static_cast<ChMaterialCompositeNSC* const>(material);
88
89 // Set different friction for left/right halfs
90 float friction = (contactinfo.vpA.z() > 0) ? 0.3f : 0.8f;
91 mat->static_friction = friction;
92 mat->sliding_friction = friction;
93 }
94 };
95
main(int argc,char * argv[])96 int main(int argc, char* argv[]) {
97 GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
98
99 // ----------------
100 // Parameters
101 // ----------------
102
103 float friction = 0.6f;
104 double collision_envelope = .001;
105
106 // -----------------
107 // Create the system
108 // -----------------
109
110 ChSystemNSC system;
111 system.Set_G_acc(ChVector<>(0, -10, 0));
112
113 // Set solver settings
114 system.SetSolverMaxIterations(100);
115 system.SetMaxPenetrationRecoverySpeed(1e8);
116 system.SetSolverForceTolerance(0);
117
118 // --------------------------------------------------
119 // Create a contact material, shared among all bodies
120 // --------------------------------------------------
121
122 auto material = chrono_types::make_shared<ChMaterialSurfaceNSC>();
123 material->SetFriction(friction);
124
125 // ----------
126 // Add bodies
127 // ----------
128
129 auto container = chrono_types::make_shared<ChBody>();
130 system.Add(container);
131 container->SetPos(ChVector<>(0, 0, 0));
132 container->SetBodyFixed(true);
133 container->SetIdentifier(-1);
134
135 container->SetCollide(true);
136 container->GetCollisionModel()->SetEnvelope(collision_envelope);
137 container->GetCollisionModel()->ClearModel();
138 utils::AddBoxGeometry(container.get(), material, ChVector<>(4, 0.5, 4), ChVector<>(0, -0.5, 0));
139 container->GetCollisionModel()->BuildModel();
140
141 container->AddAsset(chrono_types::make_shared<ChColorAsset>(ChColor(0.4f, 0.4f, 0.4f)));
142
143 auto box1 = chrono_types::make_shared<ChBody>();
144 box1->SetMass(10);
145 box1->SetInertiaXX(ChVector<>(1, 1, 1));
146 box1->SetPos(ChVector<>(-1, 0.21, -1));
147 box1->SetPos_dt(ChVector<>(5, 0, 0));
148
149 box1->SetCollide(true);
150 box1->GetCollisionModel()->SetEnvelope(collision_envelope);
151 box1->GetCollisionModel()->ClearModel();
152 utils::AddBoxGeometry(box1.get(), material, ChVector<>(0.4, 0.2, 0.1));
153 box1->GetCollisionModel()->BuildModel();
154
155 box1->AddAsset(chrono_types::make_shared<ChColorAsset>(ChColor(0.1f, 0.1f, 0.4f)));
156
157 system.AddBody(box1);
158
159 auto box2 = std::shared_ptr<ChBody>(system.NewBody());
160 box2->SetMass(10);
161 box2->SetInertiaXX(ChVector<>(1, 1, 1));
162 box2->SetPos(ChVector<>(-1, 0.21, +1));
163 box2->SetPos_dt(ChVector<>(5, 0, 0));
164
165 box2->SetCollide(true);
166 box2->GetCollisionModel()->SetEnvelope(collision_envelope);
167 box2->GetCollisionModel()->ClearModel();
168 utils::AddBoxGeometry(box2.get(), material, ChVector<>(0.4, 0.2, 0.1));
169 box2->GetCollisionModel()->BuildModel();
170
171 box2->AddAsset(chrono_types::make_shared<ChColorAsset>(ChColor(0.4f, 0.1f, 0.1f)));
172
173 system.AddBody(box2);
174
175 // -------------------------------
176 // Create the visualization window
177 // -------------------------------
178
179 irrlicht::ChIrrApp application(&system, L"NSC callbacks", irr::core::dimension2d<irr::u32>(800, 600));
180 application.AddTypicalLogo();
181 application.AddTypicalSky();
182 application.AddTypicalLights();
183 application.AddTypicalCamera(irr::core::vector3df(4, 4, -6));
184
185 application.AssetBindAll();
186 application.AssetUpdateAll();
187
188 // ---------------
189 // Simulate system
190 // ---------------
191
192 auto creporter = chrono_types::make_shared<ContactReporter>(box1, box2);
193
194 auto cmaterial = chrono_types::make_shared<ContactMaterial>();
195 system.GetContactContainer()->RegisterAddContactCallback(cmaterial);
196
197 application.SetTimestep(1e-3);
198
199 while (application.GetDevice()->run()) {
200 application.BeginScene(true, true, irr::video::SColor(255, 140, 161, 192));
201 application.DrawAll();
202 irrlicht::tools::drawGrid(application.GetVideoDriver(), 0.5, 0.5, 12, 12,
203 ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngX(CH_C_PI_2)));
204 application.DoStep();
205 application.EndScene();
206
207 // Process contacts
208 std::cout << system.GetChTime() << " " << system.GetNcontacts() << std::endl;
209 system.GetContactContainer()->ReportAllContacts(creporter);
210
211 // Cumulative contact force and torque on boxes (as applied to COM)
212 ChVector<> frc1 = box1->GetContactForce();
213 ChVector<> trq1 = box1->GetContactTorque();
214 printf(" Box 1 contact force at COM: %7.3f %7.3f %7.3f", frc1.x(), frc1.y(), frc1.z());
215 printf(" contact torque at COM: %7.3f %7.3f %7.3f\n", trq1.x(), trq1.y(), trq1.z());
216 ChVector<> frc2 = box2->GetContactForce();
217 ChVector<> trq2 = box2->GetContactTorque();
218 printf(" Box 2 contact force at COM: %7.3f %7.3f %7.3f", frc2.x(), frc2.y(), frc2.z());
219 printf(" contact torque at COM: %7.3f %7.3f %7.3f\n", trq2.x(), trq2.y(), trq2.z());
220 }
221
222 return 0;
223 }
224