Skip to content

Commit a10a0d0

Browse files
janbenderjanbender
committed
- added universal joint
- improved hinge joint - added documentation for position based fluids
1 parent 3c2217e commit a10a0d0

14 files changed

+797
-115
lines changed

‎Changelog.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
1+
- added universal joint
2+
- improved hinge joint
13
- added documentation for position based fluids
24

35
1.3.0

‎Demos/RigidBodyDemos/JointDemo.cpp‎

Lines changed: 28 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ std::vector<unsigned int> selectedBodies;
4444
Eigen::Vector3f oldMousePos;
4545
float jointColor[4] = { 0.0f, 0.4f, 0.2f, 1.0f };
4646
float dynamicBodyColor[4] = { 0.1f, 0.4f, 0.8f, 1 };
47-
float staticBodyColor[4] = { 0.6f, 0.0f, 0.0f, 1.0f };
47+
float staticBodyColor[4] = { 0.4f, 0.4f, 0.4f, 1.0f };
4848

4949
// main
5050
int main( int argc, char **argv )
@@ -61,7 +61,7 @@ int main( int argc, char **argv )
6161
buildModel ();
6262

6363
MiniGL::setClientSceneFunc(render);
64-
MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3f (4.0f, -2.5f, 15.0f), Vector3f (4.0f, 0.0f, 0.0f));
64+
MiniGL::setViewport (50.0f, 0.1f, 500.0f, Vector3f (6.0f, -2.5f, 15.0f), Vector3f (6.0f, 0.0f, 0.0f));
6565

6666
TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
6767
TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, setTimeStep, getTimeStep, &model, " label='Time step size' min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
@@ -156,9 +156,19 @@ void renderBallOnLineJoint(RigidBodyModel::BallOnLineJoint &bj)
156156

157157
void renderHingeJoint(RigidBodyModel::HingeJoint &hj)
158158
{
159-
MiniGL::drawSphere(hj.m_jointInfo.col(2), 0.1f, jointColor);
160-
MiniGL::drawSphere(hj.m_jointInfo.col(9), 0.1f, jointColor);
161-
MiniGL::drawCylinder(hj.m_jointInfo.col(2), hj.m_jointInfo.col(9), jointColor, 0.05f);
159+
MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
160+
MiniGL::drawSphere(hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
161+
MiniGL::drawCylinder(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), hj.m_jointInfo.col(6) + 0.5*hj.m_jointInfo.col(8), jointColor, 0.05f);
162+
}
163+
164+
void renderUniversalJoint(RigidBodyModel::UniversalJoint &uj)
165+
{
166+
MiniGL::drawSphere(uj.m_jointInfo.col(4) - 0.5*uj.m_jointInfo.col(6), 0.1f, jointColor);
167+
MiniGL::drawSphere(uj.m_jointInfo.col(4) + 0.5*uj.m_jointInfo.col(6), 0.1f, jointColor);
168+
MiniGL::drawSphere(uj.m_jointInfo.col(5) - 0.5*uj.m_jointInfo.col(7), 0.1f, jointColor);
169+
MiniGL::drawSphere(uj.m_jointInfo.col(5) + 0.5*uj.m_jointInfo.col(7), 0.1f, jointColor);
170+
MiniGL::drawCylinder(uj.m_jointInfo.col(4) - 0.5*uj.m_jointInfo.col(6), uj.m_jointInfo.col(4) + 0.5*uj.m_jointInfo.col(6), jointColor, 0.05f);
171+
MiniGL::drawCylinder(uj.m_jointInfo.col(5) - 0.5*uj.m_jointInfo.col(7), uj.m_jointInfo.col(5) + 0.5*uj.m_jointInfo.col(7), jointColor, 0.05f);
162172
}
163173

164174
void render ()
@@ -206,12 +216,17 @@ void render ()
206216
{
207217
renderHingeJoint(*(RigidBodyModel::HingeJoint*) joints[i]);
208218
}
219+
else if (joints[i]->getTypeId() == RigidBodyModel::UniversalJoint::TYPE_ID)
220+
{
221+
renderUniversalJoint(*(RigidBodyModel::UniversalJoint*) joints[i]);
222+
}
209223
}
210224

211225
float textColor[4] = { 0.0f, .2f, .4f, 1 };
212-
MiniGL::drawStrokeText(-0.75f, 1.5f, 1.0f, 0.003f, "BallJoint", 10, textColor);
213-
MiniGL::drawStrokeText(2.8f, 1.5f, 1.0f, 0.003f, "BallOnLineJoint", 16, textColor);
214-
MiniGL::drawStrokeText(7.1f, 1.5f, 1.0f, 0.003f, "HingeJoint", 11, textColor);
226+
MiniGL::drawStrokeText(-0.5f, 1.5f, 1.0f, 0.002f, "ball joint", 11, textColor);
227+
MiniGL::drawStrokeText(3.0f, 1.5f, 1.0f, 0.002f, "ball-on-line joint", 19, textColor);
228+
MiniGL::drawStrokeText(7.3f, 1.5f, 1.0f, 0.002f, "hinge joint", 12, textColor);
229+
MiniGL::drawStrokeText(11.2f, 1.5f, 1.0f, 0.002f, "universal joint", 15, textColor);
215230

216231
MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
217232
}
@@ -234,9 +249,9 @@ void createBodyModel()
234249
RigidBodyModel::JointVector &joints = model.getJoints();
235250

236251
// static body
237-
rb.resize(9);
252+
rb.resize(12);
238253
float startX = 0.0f;
239-
for (unsigned int i = 0; i < 3; i++)
254+
for (unsigned int i = 0; i < 4; i++)
240255
{
241256
rb[3*i] = new RigidBody();
242257
rb[3*i]->initBody(0.0f,
@@ -270,6 +285,9 @@ void createBodyModel()
270285

271286
model.addHingeJoint(6, 7, Eigen::Vector3f(8.0f, 0.75f, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
272287
model.addBallJoint(7, 8, Eigen::Vector3f(8.25f, 0.75f, 3.0f));
288+
289+
model.addUniversalJoint(9, 10, Eigen::Vector3f(12.0f, 0.75f, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
290+
model.addBallJoint(10, 11, Eigen::Vector3f(12.25f, 0.75f, 3.0f));
273291
}
274292

275293
void TW_CALL setTimeStep(const void *value, void *clientData)

‎Demos/RigidBodyDemos/RigidBodyModel.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ using namespace PBD;
66
int RigidBodyModel::BallJoint::TYPE_ID = 1;
77
int RigidBodyModel::BallOnLineJoint::TYPE_ID = 2;
88
int RigidBodyModel::HingeJoint::TYPE_ID = 3;
9+
int RigidBodyModel::UniversalJoint::TYPE_ID = 4;
910

1011
RigidBodyModel::RigidBodyModel()
1112
{
@@ -65,9 +66,12 @@ void RigidBodyModel::updateJoints()
6566
updateBallOnLineJoint(i);
6667
else if (m_joints[i]->getTypeId() == RigidBodyModel::HingeJoint::TYPE_ID)
6768
updateHingeJoint(i);
69+
else if (m_joints[i]->getTypeId() == RigidBodyModel::UniversalJoint::TYPE_ID)
70+
updateUniversalJoint(i);
6871
}
6972
}
7073

74+
7175
void RigidBodyModel::addBallJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos)
7276
{
7377
BallJoint *bj = new BallJoint();
@@ -136,6 +140,7 @@ void RigidBodyModel::updateBallOnLineJoint(const unsigned int i)
136140
bj.m_jointInfo);
137141
}
138142

143+
139144
void RigidBodyModel::addHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
140145
{
141146
HingeJoint *hj = new HingeJoint();
@@ -170,3 +175,39 @@ void RigidBodyModel::updateHingeJoint(const unsigned int i)
170175
rb2.getRotation(),
171176
hj.m_jointInfo);
172177
}
178+
179+
void RigidBodyModel::addUniversalJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis1, const Eigen::Vector3f &axis2)
180+
{
181+
UniversalJoint *uj = new UniversalJoint();
182+
uj->m_index[0] = rbIndex1;
183+
uj->m_index[1] = rbIndex2;
184+
185+
// transform in local coordinates
186+
RigidBody &rb1 = *m_rigidBodies[rbIndex1];
187+
RigidBody &rb2 = *m_rigidBodies[rbIndex2];
188+
189+
PositionBasedRigidBodyDynamics::initRigidBodyUniversalJointInfo(
190+
rb1.getPosition0(),
191+
rb1.getRotation0(),
192+
rb2.getPosition0(),
193+
rb2.getRotation0(),
194+
pos,
195+
axis1,
196+
axis2,
197+
uj->m_jointInfo);
198+
199+
m_joints.push_back(uj);
200+
}
201+
202+
void RigidBodyModel::updateUniversalJoint(const unsigned int i)
203+
{
204+
UniversalJoint &uj = *(UniversalJoint*)m_joints[i];
205+
RigidBody &rb1 = *m_rigidBodies[uj.m_index[0]];
206+
RigidBody &rb2 = *m_rigidBodies[uj.m_index[1]];
207+
PositionBasedRigidBodyDynamics::updateRigidBodyUniversalJointInfo(
208+
rb1.getPosition(),
209+
rb1.getRotation(),
210+
rb2.getPosition(),
211+
rb2.getRotation(),
212+
uj.m_jointInfo);
213+
}

‎Demos/RigidBodyDemos/RigidBodyModel.h‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,12 +40,22 @@ namespace PBD
4040
struct HingeJoint : public Joint
4141
{
4242
/** joint points in local coordinates */
43-
Eigen::Matrix<float, 3, 14> m_jointInfo;
43+
Eigen::Matrix<float, 3, 12> m_jointInfo;
4444

4545
static int TYPE_ID;
4646
virtual int &getTypeId() const { return TYPE_ID; }
4747
};
4848

49+
struct UniversalJoint : public Joint
50+
{
51+
/** joint points in local coordinates */
52+
Eigen::Matrix<float, 3, 8> m_jointInfo;
53+
54+
static int TYPE_ID;
55+
virtual int &getTypeId() const { return TYPE_ID; }
56+
};
57+
58+
4959
RigidBodyModel();
5060
virtual ~RigidBodyModel();
5161

@@ -72,6 +82,9 @@ namespace PBD
7282

7383
void addHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
7484
void updateHingeJoint(const unsigned int i);
85+
86+
void addUniversalJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis1, const Eigen::Vector3f &axis2);
87+
void updateUniversalJoint(const unsigned int i);
7588
};
7689
}
7790

‎Demos/RigidBodyDemos/TimeStepRigidBodyModel.cpp‎

Lines changed: 48 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -88,17 +88,13 @@ void TimeStepRigidBodyModel::constraintProjection(RigidBodyModel &model)
8888
for (unsigned int i = 0; i < joints.size(); i++)
8989
{
9090
if (joints[i]->getTypeId() == RigidBodyModel::BallJoint::TYPE_ID)
91-
{
9291
constraintProjectionBallJoint(model, i);
93-
}
9492
else if (joints[i]->getTypeId() == RigidBodyModel::BallOnLineJoint::TYPE_ID)
95-
{
9693
constraintProjectionBallOnLineJoint(model, i);
97-
}
9894
else if (joints[i]->getTypeId() == RigidBodyModel::HingeJoint::TYPE_ID)
99-
{
10095
constraintProjectionHingeJoint(model, i);
101-
}
96+
else if (joints[i]->getTypeId() == RigidBodyModel::UniversalJoint::TYPE_ID)
97+
constraintProjectionUniversalJoint(model, i);
10298
}
10399
iter++;
104100
}
@@ -245,3 +241,49 @@ void TimeStepRigidBodyModel::constraintProjectionHingeJoint(RigidBodyModel &mode
245241
}
246242
}
247243

244+
void TimeStepRigidBodyModel::constraintProjectionUniversalJoint(RigidBodyModel &model, const unsigned int index)
245+
{
246+
RigidBodyModel::JointVector &joints = model.getJoints();
247+
RigidBodyModel::RigidBodyVector &rb = model.getRigidBodies();
248+
RigidBodyModel::UniversalJoint &uj = *(RigidBodyModel::UniversalJoint*) joints[index];
249+
250+
model.updateUniversalJoint(index);
251+
252+
RigidBody &rb1 = *rb[uj.m_index[0]];
253+
RigidBody &rb2 = *rb[uj.m_index[1]];
254+
255+
Eigen::Vector3f corr_x1, corr_x2;
256+
Eigen::Quaternionf corr_q1, corr_q2;
257+
const bool res = PositionBasedRigidBodyDynamics::solveRigidBodyUniversalJoint(
258+
rb1.getMass(),
259+
rb1.getPosition(),
260+
rb1.getInertiaTensorInverseW(),
261+
rb1.getRotation(),
262+
rb2.getMass(),
263+
rb2.getPosition(),
264+
rb2.getInertiaTensorInverseW(),
265+
rb2.getRotation(),
266+
uj.m_jointInfo,
267+
corr_x1,
268+
corr_q1,
269+
corr_x2,
270+
corr_q2);
271+
272+
if (res)
273+
{
274+
if (rb1.getMass() != 0.0f)
275+
{
276+
rb1.getPosition() += corr_x1;
277+
rb1.getRotation().coeffs() += corr_q1.coeffs();
278+
rb1.getRotation().normalize();
279+
rb1.rotationUpdated();
280+
}
281+
if (rb2.getMass() != 0.0f)
282+
{
283+
rb2.getPosition() += corr_x2;
284+
rb2.getRotation().coeffs() += corr_q2.coeffs();
285+
rb2.getRotation().normalize();
286+
rb2.rotationUpdated();
287+
}
288+
}
289+
}

‎Demos/RigidBodyDemos/TimeStepRigidBodyModel.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ namespace PBD
1818
void constraintProjectionBallJoint(RigidBodyModel &model, const unsigned int index);
1919
void constraintProjectionBallOnLineJoint(RigidBodyModel &model, const unsigned int index);
2020
void constraintProjectionHingeJoint(RigidBodyModel &model, const unsigned int index);
21+
void constraintProjectionUniversalJoint(RigidBodyModel &model, const unsigned int index);
2122

2223
public:
2324
TimeStepRigidBodyModel();

‎PositionBasedDynamics/MathFunctions.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -395,3 +395,11 @@ float MathFunctions::cotTheta(const Eigen::Vector3f &v, const Eigen::Vector3f &w
395395
return (cosTheta / sinTheta);
396396
}
397397

398+
// ----------------------------------------------------------------------------------------------
399+
void MathFunctions::crossProductMatrix(const Eigen::Vector3f &v, Eigen::Matrix3f &v_hat)
400+
{
401+
v_hat << 0, -v(2), v(1),
402+
v(2), 0, -v(0),
403+
-v(1), v(0), 0;
404+
}
405+

‎PositionBasedDynamics/MathFunctions.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,12 @@ namespace PBD
3737
Eigen::Matrix3f &VT);
3838

3939
static float cotTheta(const Eigen::Vector3f &v, const Eigen::Vector3f &w);
40+
41+
/** Computes the cross product matrix of a vector.
42+
* @param v input vector
43+
* @param v_hat resulting cross product matrix
44+
*/
45+
static void crossProductMatrix(const Eigen::Vector3f &v, Eigen::Matrix3f &v_hat);
4046
};
4147
}
4248

‎PositionBasedDynamics/PositionBasedDynamics.cpp‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -104,9 +104,6 @@ bool PositionBasedDynamics::solveDihedralConstraint(
104104
return true;
105105
}
106106

107-
/** Return the position corrections for a tetrahedral volume constraint,
108-
* where the tetrahedron (p0,p1,p2,p3) has the given rest volume.
109-
*/
110107
bool PositionBasedDynamics::solveVolumeConstraint(
111108
const Eigen::Vector3f &p0, float invMass0,
112109
const Eigen::Vector3f &p1, float invMass1,

0 commit comments

Comments
 (0)