FrictionJoint.cpp
Go to the documentation of this file.
1 /*
2  * Original work Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
3  * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
4  *
5  * This software is provided 'as-is', without any express or implied
6  * warranty. In no event will the authors be held liable for any damages
7  * arising from the use of this software.
8  *
9  * Permission is granted to anyone to use this software for any purpose,
10  * including commercial applications, and to alter it and redistribute it
11  * freely, subject to the following restrictions:
12  *
13  * 1. The origin of this software must not be misrepresented; you must not
14  * claim that you wrote the original software. If you use this software
15  * in a product, an acknowledgment in the product documentation would be
16  * appreciated but is not required.
17  * 2. Altered source versions must be plainly marked as such, and must not be
18  * misrepresented as being the original software.
19  * 3. This notice may not be removed or altered from any source distribution.
20  */
21 
22 #include <PlayRho/Dynamics/Joints/FrictionJoint.hpp>
23 #include <PlayRho/Dynamics/Joints/JointVisitor.hpp>
24 #include <PlayRho/Dynamics/Body.hpp>
25 #include <PlayRho/Dynamics/StepConf.hpp>
26 #include <PlayRho/Dynamics/Contacts/BodyConstraint.hpp>
27 
28 #include <algorithm>
29 
30 namespace playrho {
31 namespace d2 {
32 
33 // Point-to-point constraint
34 // Cdot = v2 - v1
35 // = v2 + cross(w2, r2) - v1 - cross(w1, r1)
36 // J = [-I -r1_skew I r2_skew ]
37 // Identity used:
38 // w k % (rx i + ry j) = w * (-ry i + rx j)
39 
40 // Angle constraint
41 // Cdot = w2 - w1
42 // J = [0 0 -1 0 0 1]
43 // K = invI1 + invI2
44 
45 FrictionJoint::FrictionJoint(const FrictionJointConf& def):
46  Joint(def),
47  m_localAnchorA(def.localAnchorA),
48  m_localAnchorB(def.localAnchorB),
49  m_maxForce(def.maxForce),
50  m_maxTorque(def.maxTorque)
51 {
52  // Intentionally empty.
53 }
54 
55 void FrictionJoint::Accept(JointVisitor& visitor) const
56 {
57  visitor.Visit(*this);
58 }
59 
60 void FrictionJoint::Accept(JointVisitor& visitor)
61 {
62  visitor.Visit(*this);
63 }
64 
65 void FrictionJoint::InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step,
66  const ConstraintSolverConf&)
67 {
68  auto& bodyConstraintA = At(bodies, GetBodyA());
69  auto& bodyConstraintB = At(bodies, GetBodyB());
70  const auto posA = bodyConstraintA->GetPosition();
71  auto velA = bodyConstraintA->GetVelocity();
72  const auto posB = bodyConstraintB->GetPosition();
73  auto velB = bodyConstraintB->GetVelocity();
74 
75  // Compute the effective mass matrix.
76  m_rA = Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), UnitVec::Get(posA.angular));
77  m_rB = Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), UnitVec::Get(posB.angular));
78 
79  // J = [-I -r1_skew I r2_skew]
80  // [ 0 -1 0 1]
81  // r_skew = [-ry; rx]
82 
83  // Matlab
84  // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
85  // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
86  // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
87 
88  const auto invMassA = bodyConstraintA->GetInvMass();
89  const auto invMassB = bodyConstraintB->GetInvMass();
90  const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
91  const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
92 
93  {
94  const auto exx = InvMass{
95  invMassA + invRotInertiaA * Square(GetY(m_rA)) / SquareRadian +
96  invMassB + invRotInertiaB * Square(GetY(m_rB)) / SquareRadian
97  };
98  const auto exy = InvMass{
99  -invRotInertiaA * GetX(m_rA) * GetY(m_rA) / SquareRadian +
100  -invRotInertiaB * GetX(m_rB) * GetY(m_rB) / SquareRadian
101  };
102  const auto eyy = InvMass{
103  invMassA + invRotInertiaA * Square(GetX(m_rA)) / SquareRadian +
104  invMassB + invRotInertiaB * Square(GetX(m_rB)) / SquareRadian
105  };
106  InvMass22 K;
107  GetX(GetX(K)) = exx;
108  GetY(GetX(K)) = exy;
109  GetX(GetY(K)) = exy;
110  GetY(GetY(K)) = eyy;
111  m_linearMass = Invert(K);
112  }
113 
114  const auto invRotInertia = invRotInertiaA + invRotInertiaB;
115  m_angularMass = (invRotInertia > InvRotInertia{0})?
116  RotInertia{Real{1} / invRotInertia}: RotInertia{0};
117 
118  if (step.doWarmStart)
119  {
120  // Scale impulses to support a variable time step.
121  m_linearImpulse *= step.dtRatio;
122  m_angularImpulse *= step.dtRatio;
123 
124  const auto P = m_linearImpulse;
125 
126  // L * M * L T^-1 / QP is: L^2 M T^-1 QP^-1 which is: AngularMomentum.
127  const auto crossAP = AngularMomentum{Cross(m_rA, P) / Radian};
128  const auto crossBP = AngularMomentum{Cross(m_rB, P) / Radian}; // L * M * L T^-1 is: L^2 M T^-1
129 
130  velA -= Velocity{invMassA * P, invRotInertiaA * (crossAP + m_angularImpulse)};
131  velB += Velocity{invMassB * P, invRotInertiaB * (crossBP + m_angularImpulse)};
132  }
133  else
134  {
135  m_linearImpulse = Momentum2{};
136  m_angularImpulse = AngularMomentum{0};
137  }
138 
139  bodyConstraintA->SetVelocity(velA);
140  bodyConstraintB->SetVelocity(velB);
141 }
142 
143 bool FrictionJoint::SolveVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step)
144 {
145  auto& bodyConstraintA = At(bodies, GetBodyA());
146  auto& bodyConstraintB = At(bodies, GetBodyB());
147 
148  auto velA = bodyConstraintA->GetVelocity();
149  const auto invRotInertiaA = bodyConstraintA->GetInvRotInertia();
150 
151  auto velB = bodyConstraintB->GetVelocity();
152  const auto invRotInertiaB = bodyConstraintB->GetInvRotInertia();
153 
154  const auto h = step.GetTime();
155 
156  auto solved = true;
157 
158  // Solve angular friction
159  {
160  // L^2 M QP^-2 * QP T^-1 is: L^2 M QP^-1 T^-1 (SquareMeter * Kilogram / Second) / Radian
161  // L^2 M QP^-1 T^-1
162  const auto angularImpulse = AngularMomentum{-m_angularMass * (velB.angular - velA.angular)};
163 
164  const auto oldAngularImpulse = m_angularImpulse;
165  const auto maxAngularImpulse = h * m_maxTorque;
166  m_angularImpulse = std::clamp(m_angularImpulse + angularImpulse,
167  -maxAngularImpulse, maxAngularImpulse);
168  const auto incAngularImpulse = m_angularImpulse - oldAngularImpulse;
169 
170  if (incAngularImpulse != AngularMomentum{0})
171  {
172  solved = false;
173  }
174 
175  velA.angular -= invRotInertiaA * incAngularImpulse;
176  velB.angular += invRotInertiaB * incAngularImpulse;
177  }
178 
179  // Solve linear friction
180  {
181  const auto vb = LinearVelocity2{velB.linear + (GetRevPerpendicular(m_rB) * (velB.angular / Radian))};
182  const auto va = LinearVelocity2{velA.linear + (GetRevPerpendicular(m_rA) * (velA.angular / Radian))};
183 
184  const auto impulse = -Transform(vb - va, m_linearMass);
185  const auto oldImpulse = m_linearImpulse;
186  m_linearImpulse += impulse;
187 
188  const auto maxImpulse = h * m_maxForce;
189 
190  if (GetMagnitudeSquared(m_linearImpulse) > Square(maxImpulse))
191  {
192  m_linearImpulse = GetUnitVector(m_linearImpulse, UnitVec::GetZero()) * maxImpulse;
193  }
194 
195  const auto incImpulse = Momentum2{m_linearImpulse - oldImpulse};
196  const auto angImpulseA = AngularMomentum{Cross(m_rA, incImpulse) / Radian};
197  const auto angImpulseB = AngularMomentum{Cross(m_rB, incImpulse) / Radian};
198 
199  if (incImpulse != Momentum2{})
200  {
201  solved = false;
202  }
203 
204  velA -= Velocity{bodyConstraintA->GetInvMass() * incImpulse, invRotInertiaA * angImpulseA};
205  velB += Velocity{bodyConstraintB->GetInvMass() * incImpulse, invRotInertiaB * angImpulseB};
206  }
207 
208  bodyConstraintA->SetVelocity(velA);
209  bodyConstraintB->SetVelocity(velB);
210 
211  return solved;
212 }
213 
214 bool FrictionJoint::SolvePositionConstraints(BodyConstraintsMap&, const ConstraintSolverConf&) const
215 {
216  return true;
217 }
218 
219 Length2 FrictionJoint::GetAnchorA() const
220 {
221  return GetWorldPoint(*GetBodyA(), GetLocalAnchorA());
222 }
223 
224 Length2 FrictionJoint::GetAnchorB() const
225 {
226  return GetWorldPoint(*GetBodyB(), GetLocalAnchorB());
227 }
228 
229 Momentum2 FrictionJoint::GetLinearReaction() const
230 {
231  return m_linearImpulse;
232 }
233 
234 AngularMomentum FrictionJoint::GetAngularReaction() const
235 {
236  return m_angularImpulse;
237 }
238 
239 } // namespace d2
240 } // namespace playrho