VelocityConstraint.cpp
Go to the documentation of this file.
1 /*
2  * Original work Copyright (c) 2006-2009 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  * Permission is granted to anyone to use this software for any purpose,
9  * including commercial applications, and to alter it and redistribute it
10  * freely, subject to the following restrictions:
11  * 1. The origin of this software must not be misrepresented; you must not
12  * claim that you wrote the original software. If you use this software
13  * in a product, an acknowledgment in the product documentation would be
14  * appreciated but is not required.
15  * 2. Altered source versions must be plainly marked as such, and must not be
16  * misrepresented as being the original software.
17  * 3. This notice may not be removed or altered from any source distribution.
18  */
19 
20 #include <PlayRho/Dynamics/Contacts/VelocityConstraint.hpp>
21 #include <PlayRho/Dynamics/StepConf.hpp>
22 #include <PlayRho/Collision/WorldManifold.hpp>
23 #include <PlayRho/Collision/Manifold.hpp>
24 
25 #define PLAYRHO_MAGIC(x) (x)
26 
27 namespace playrho {
28 namespace d2 {
29 
30 namespace {
31 
32 inline InvMass3 ComputeK(const VelocityConstraint& vc) noexcept
33 {
34  assert(vc.GetPointCount() == 2);
35 
36  const auto normal = vc.GetNormal();
37  const auto bodyA = vc.GetBodyA();
38  const auto bodyB = vc.GetBodyB();
39 
40  const auto relA0 = vc.GetPointRelPosA(0);
41  const auto relB0 = vc.GetPointRelPosB(0);
42  const auto relA1 = vc.GetPointRelPosA(1);
43  const auto relB1 = vc.GetPointRelPosB(1);
44 
45  const auto rnA0 = Length{Cross(relA0, normal)};
46  const auto rnB0 = Length{Cross(relB0, normal)};
47  const auto rnA1 = Length{Cross(relA1, normal)};
48  const auto rnB1 = Length{Cross(relB1, normal)};
49 
50  const auto invRotInertiaA = bodyA->GetInvRotInertia();
51  const auto invMassA = bodyA->GetInvMass();
52 
53  const auto invRotInertiaB = bodyB->GetInvRotInertia();
54  const auto invMassB = bodyB->GetInvMass();
55 
56  const auto invMass = invMassA + invMassB;
57  assert(invMass > InvMass{0});
58 
59  const auto invRotMassA0 = InvMass{(invRotInertiaA * Square(rnA0)) / SquareRadian};
60  const auto invRotMassA1 = InvMass{(invRotInertiaA * Square(rnA1)) / SquareRadian};
61  const auto invRotMassB0 = InvMass{(invRotInertiaB * Square(rnB0)) / SquareRadian};
62  const auto invRotMassB1 = InvMass{(invRotInertiaB * Square(rnB1)) / SquareRadian};
63  const auto invRotMassA = InvMass{(invRotInertiaA * rnA0 * rnA1) / SquareRadian};
64  const auto invRotMassB = InvMass{(invRotInertiaB * rnB0 * rnB1) / SquareRadian};
65 
66  const auto k00 = invMass + invRotMassA0 + invRotMassB0;
67  const auto k11 = invMass + invRotMassA1 + invRotMassB1;
68  const auto k01 = invMass + invRotMassA + invRotMassB;
69 
70  return InvMass3{k00, k11, k01};
71 }
72 
73 } // anonymous namespace
74 
75 VelocityConstraint::VelocityConstraint(Real friction, Real restitution,
76  LinearVelocity tangentSpeed,
77  const WorldManifold& worldManifold,
78  BodyConstraint& bA,
79  BodyConstraint& bB,
80  Conf conf):
81  m_bodyA{&bA}, m_bodyB{&bB},
82  m_normal{worldManifold.GetNormal()},
83  m_friction{friction}, m_restitution{restitution}, m_tangentSpeed{tangentSpeed}
84 {
85  assert(IsValid(friction));
86  assert(IsValid(restitution));
87  assert(IsValid(tangentSpeed));
88  assert(IsValid(m_normal));
89 
90  const auto pointCount = worldManifold.GetPointCount();
91  assert(pointCount > 0);
92  for (auto j = decltype(pointCount){0}; j < pointCount; ++j)
93  {
94  const auto ci = worldManifold.GetImpulses(j);
95  const auto worldPoint = worldManifold.GetPoint(j);
96  const auto relA = worldPoint - bA.GetPosition().linear;
97  const auto relB = worldPoint - bB.GetPosition().linear;
98  AddPoint(get<0>(ci), get<1>(ci), relA, relB, conf);
99  }
100 
101  if (conf.blockSolve && (pointCount == 2))
102  {
103  const auto k = ComputeK(*this);
104 
105  // Ensure a reasonable condition number.
106  const auto k00_squared = Square(get<0>(k));
107  const auto k00_times_k11 = get<0>(k) * get<1>(k);
108  const auto k01_squared = Square(get<2>(k));
109  const auto k_diff = k00_times_k11 - k01_squared;
110  PLAYRHO_CONSTEXPR const auto maxCondNum = PLAYRHO_MAGIC(Real(1000.0f));
111  if (k00_squared < maxCondNum * k_diff)
112  {
113  // K is safe to invert.
114  // Prepare the block solver.
115  m_K = k;
116  const auto normalMass = Invert(InvMass22{InvMass2{get<0>(k), get<2>(k)}, InvMass2{get<2>(k), get<1>(k)}});
117  m_normalMass = Mass3{get<0>(get<0>(normalMass)), get<1>(get<1>(normalMass)), get<1>(get<0>(normalMass))};
118  }
119  else
120  {
121  // The constraints are redundant, just use one.
122  // TODO(lou) use deepest?
123  RemovePoint();
124  }
125  }
126 }
127 
128 VelocityConstraint::Point
129 VelocityConstraint::GetPoint(Momentum normalImpulse, Momentum tangentImpulse,
130  Length2 relA, Length2 relB, Conf conf) const noexcept
131 {
132  assert(IsValid(normalImpulse));
133  assert(IsValid(tangentImpulse));
134  assert(IsValid(relA));
135  assert(IsValid(relB));
136 
137  const auto bodyA = GetBodyA();
138  const auto bodyB = GetBodyB();
139 
140  const auto invRotInertiaA = bodyA->GetInvRotInertia();
141  const auto invMassA = bodyA->GetInvMass();
142  const auto invRotInertiaB = bodyB->GetInvRotInertia();
143  const auto invMassB = bodyB->GetInvMass();
144  const auto invMass = invMassA + invMassB;
145 
146  Point point;
147  point.normalImpulse = normalImpulse;
148  point.tangentImpulse = tangentImpulse;
149  point.velocityBias = [&]() {
150  // Get the magnitude of the contact relative velocity in direction of the normal.
151  // This will be an invalid value if the normal is invalid. The comparison in this
152  // case will fail and this lambda will return 0. And that's fine. There's no need
153  // to have a check that the normal is valid and possibly incur the overhead of a
154  // conditional branch here.
155  const auto dv = GetContactRelVelocity(bodyA->GetVelocity(), relA,
156  bodyB->GetVelocity(), relB);
157  const auto vn = LinearVelocity{Dot(dv, GetNormal())};
158  return (vn < -conf.velocityThreshold)? -GetRestitution() * vn: 0_mps;
159  }();
160  point.relA = relA;
161  point.relB = relB;
162  point.normalMass = [&](){
163  const auto invRotMassA = invRotInertiaA * Square(Cross(relA, GetNormal())) / SquareRadian;
164  const auto invRotMassB = invRotInertiaB * Square(Cross(relB, GetNormal())) / SquareRadian;
165  const auto value = invMass + invRotMassA + invRotMassB;
166  return (value != InvMass{0})? Real{1} / value : 0_kg;
167  }();
168  point.tangentMass = [&]() {
169  const auto invRotMassA = invRotInertiaA * Square(Cross(relA, GetTangent())) / SquareRadian;
170  const auto invRotMassB = invRotInertiaB * Square(Cross(relB, GetTangent())) / SquareRadian;
171  const auto value = invMass + invRotMassA + invRotMassB;
172  return (value != InvMass{0})? Real{1} / value : 0_kg;
173  }();
174 
175  return point;
176 }
177 
178 void VelocityConstraint::AddPoint(Momentum normalImpulse, Momentum tangentImpulse,
179  Length2 relA, Length2 relB, Conf conf)
180 {
181  assert(m_pointCount < MaxManifoldPoints);
182  m_points[m_pointCount] = GetPoint(normalImpulse * conf.dtRatio, tangentImpulse * conf.dtRatio,
183  relA, relB, conf);
184  ++m_pointCount;
185 }
186 
187 VelocityConstraint::Conf GetRegVelocityConstraintConf(const StepConf& conf) noexcept
188 {
189  return VelocityConstraint::Conf{
190  conf.doWarmStart? conf.dtRatio: 0,
191  conf.velocityThreshold,
192  conf.doBlocksolve
193  };
194 }
195 
196 VelocityConstraint::Conf GetToiVelocityConstraintConf(const StepConf& conf) noexcept
197 {
198  return VelocityConstraint::Conf{0, conf.velocityThreshold, conf.doBlocksolve};
199 }
200 
201 } // namespace d2
202 } // namespace playrho