Toggle navigation
PlayRho
0.9.0
GitHub
Main Page
Modules
Classes
Files
Examples
File List
File Members
Dynamics
Island.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
* 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/Collision/Distance.hpp
>
21
#include <
PlayRho/Dynamics/Island.hpp
>
22
#include <
PlayRho/Dynamics/Body.hpp
>
23
#include <
PlayRho/Dynamics/Fixture.hpp
>
24
#include <
PlayRho/Dynamics/World.hpp
>
25
#include <
PlayRho/Dynamics/Contacts/Contact.hpp
>
26
27
#include <algorithm>
28
29
/*
30
Position Correction Notes
31
=========================
32
I tried the several algorithms for position correction of the 2-D revolute joint.
33
I looked at these systems:
34
- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
35
- suspension bridge with 30 1m long planks of length 1m.
36
- multi-link chain with 30 1m long links.
37
38
Here are the algorithms:
39
40
Baumgarte - A fraction of the position error is added to the velocity error. There is no
41
separate position solver.
42
43
Pseudo Velocities - After the velocity solver and position integration,
44
the position error, Jacobian, and effective mass are recomputed. Then
45
the velocity constraints are solved with pseudo velocities and a fraction
46
of the position error is added to the pseudo velocity error. The pseudo
47
velocities are initialized to zero and there is no warm-starting. After
48
the position solver, the pseudo velocities are added to the positions.
49
This is also called the First Order World method or the Position LCP method.
50
51
Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
52
position error is re-computed for each constraint and the positions are updated
53
after the constraint is solved. The radius vectors (aka Jacobians) are
54
re-computed too (otherwise the algorithm has horrible instability). The pseudo
55
velocity states are not needed because they are effectively zero at the beginning
56
of each iteration. Since we have the current position error, we allow the
57
iterations to terminate early if the error becomes smaller than the linear slop.
58
59
Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
60
each time a constraint is solved.
61
62
Here are the results:
63
Baumgarte - this is the cheapest algorithm but it has some stability problems,
64
especially with the bridge. The chain links separate easily close to the root
65
and they jitter as they struggle to pull together. This is one of the most common
66
methods in the field. The big drawback is that the position correction artificially
67
affects the momentum, thus leading to instabilities and false bounce. I used a
68
bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
69
factor makes joints and contacts more spongy.
70
71
Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
72
stable. However, joints still separate with large angular velocities. Drag the
73
simple pendulum in a circle quickly and the joint will separate. The chain separates
74
easily and does not recover. I used a bias factor of 0.2. A larger value lead to
75
the bridge collapsing when a heavy cube drops on it.
76
77
Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
78
Velocities, but in other ways it is worse. The bridge and chain are much more
79
stable, but the simple pendulum goes unstable at high angular velocities.
80
81
Full NGS - stable in all tests. The joints display good stiffness. The bridge
82
still sags, but this is better than infinite forces.
83
84
Recommendations
85
Pseudo Velocities are not really worthwhile because the bridge and chain cannot
86
recover from joint separation. In other cases the benefit over Baumgarte is small.
87
88
Modified NGS is not a robust method for the revolute joint due to the violent
89
instability seen in the simple pendulum. Perhaps it is viable with other constraint
90
types, especially scalar constraints where the effective mass is a scalar.
91
92
This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
93
and is very fast. I don't think we can escape Baumgarte, especially in highly
94
demanding cases where high constraint fidelity is not needed.
95
96
Full NGS is robust and easy on the eyes. I recommend this as an option for
97
higher fidelity simulation and certainly for suspension bridges and long chains.
98
Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
99
joint separation can be problematic. The number of NGS iterations can be reduced
100
for better performance without harming robustness much.
101
102
Each joint in a can be handled differently in the position solver. So I recommend
103
a system where the user can select the algorithm on a per joint basis. I would
104
probably default to the slower Full NGS and let the user select the faster
105
Baumgarte method in performance critical scenarios.
106
*/
107
108
/*
109
Cache Performance
110
111
The PlayRho solvers are dominated by cache misses. Data structures are designed
112
to increase the number of cache hits. Much of misses are due to random access
113
to body data. The constraint structures are iterated over linearly, which leads
114
to few cache misses.
115
116
The bodies are not accessed during iteration. Instead read only data, such as
117
the mass values are stored with the constraints. The mutable data are the constraint
118
impulses and the bodies velocities/positions. The impulses are held inside the
119
constraint structures. The body velocities/positions are held in compact, temporary
120
arrays to increase the number of cache hits. Linear and angular velocity are
121
stored in a single array since multiple arrays lead to multiple misses.
122
*/
123
124
namespace
playrho
{
125
namespace
d2 {
126
127
using
std::count;
128
129
Island::Island
(Bodies::size_type bodyCapacity,
130
Contacts::size_type contactCapacity,
131
Joints::size_type jointCapacity)
132
{
133
m_bodies
.reserve(bodyCapacity);
134
m_contacts
.reserve(contactCapacity);
135
m_joints
.reserve(jointCapacity);
136
}
137
138
std::size_t
Count
(
const
Island
& island,
const
Body
* entry)
139
{
140
return
MakeUnsigned
(count(cbegin(island.
m_bodies
), cend(island.
m_bodies
), entry));
141
}
142
143
std::size_t
Count
(
const
Island
& island,
const
Contact
* entry)
144
{
145
return
MakeUnsigned
(count(cbegin(island.
m_contacts
), cend(island.
m_contacts
), entry));
146
}
147
148
std::size_t
Count
(
const
Island
& island,
const
Joint
* entry)
149
{
150
return
MakeUnsigned
(count(cbegin(island.
m_joints
), cend(island.
m_joints
), entry));
151
}
152
153
}
// namespace d2
154
}
// namespace playrho