Skip to content

Commit 402cb57

Browse files
janbenderjanbender
committed
- added slider joint
- extended JointDemo
1 parent 188348a commit 402cb57

File tree

11 files changed

+503
-35
lines changed

11 files changed

+503
-35
lines changed

‎Changelog.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,6 @@
1+
- added slider joint
2+
- extended JointDemo
3+
14
1.4.0
25
- extended JointDemo
36
- added TargetVelocityMotorHingeJoint

‎Demos/RigidBodyDemos/JointDemo.cpp‎

Lines changed: 41 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ int main( int argc, char **argv )
5353
REPORT_MEMORY_LEAKS
5454

5555
// OpenGL
56-
MiniGL::init (argc, argv, 1024, 768, 0, 0, "Rigid body demo");
56+
MiniGL::init (argc, argv, 1280, 960, 0, 0, "Rigid body demo");
5757
MiniGL::initLights ();
5858
MiniGL::setClientIdleFunc (50, timeStep);
5959
MiniGL::setKeyFunc(0, 'r', reset);
@@ -62,7 +62,7 @@ int main( int argc, char **argv )
6262
buildModel ();
6363

6464
MiniGL::setClientSceneFunc(render);
65-
MiniGL::setViewport (50.0f, 0.1f, 500.0f, Vector3f (8.0f, -2.5f, 17.0f), Vector3f (8.0f, 0.0f, 0.0f));
65+
MiniGL::setViewport (60.0f, 0.1f, 500.0f, Vector3f (6.0f, -5.5f, 15.0f), Vector3f (6.0f, -3.0f, 0.0f));
6666

6767
TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
6868
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 ");
@@ -183,6 +183,11 @@ void renderUniversalJoint(UniversalJoint &uj)
183183
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);
184184
}
185185

186+
void renderSliderJoint(SliderJoint &joint)
187+
{
188+
MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(11), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(11), jointColor, 0.05f);
189+
}
190+
186191
void renderTargetAngleMotorHingeJoint(TargetAngleMotorHingeJoint &hj)
187192
{
188193
MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
@@ -246,6 +251,10 @@ void render ()
246251
{
247252
renderUniversalJoint(*(UniversalJoint*)constraints[i]);
248253
}
254+
else if (constraints[i]->getTypeId() == SliderJoint::TYPE_ID)
255+
{
256+
renderSliderJoint(*(SliderJoint*)constraints[i]);
257+
}
249258
else if (constraints[i]->getTypeId() == TargetAngleMotorHingeJoint::TYPE_ID)
250259
{
251260
renderTargetAngleMotorHingeJoint(*(TargetAngleMotorHingeJoint*)constraints[i]);
@@ -261,7 +270,9 @@ void render ()
261270
MiniGL::drawStrokeText(3.0f, 1.5f, 1.0f, 0.002f, "ball-on-line joint", 19, textColor);
262271
MiniGL::drawStrokeText(7.3f, 1.5f, 1.0f, 0.002f, "hinge joint", 12, textColor);
263272
MiniGL::drawStrokeText(11.2f, 1.5f, 1.0f, 0.002f, "universal joint", 15, textColor);
264-
MiniGL::drawStrokeText(15.0f, 1.5f, 1.0f, 0.002f, "motor hinge joint", 17, textColor);
273+
274+
MiniGL::drawStrokeText(-1.0f, -4.0f, 1.0f, 0.002f, "motor hinge joint", 17, textColor);
275+
MiniGL::drawStrokeText(3.4f, -4.0f, 1.0f, 0.002f, "slider joint", 12, textColor);
265276

266277
MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
267278
}
@@ -283,50 +294,61 @@ void createBodyModel()
283294
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
284295

285296
// static body
286-
rb.resize(15);
297+
rb.resize(18);
287298
float startX = 0.0f;
288-
for (unsigned int i = 0; i < 5; i++)
299+
float startY = 1.0f;
300+
for (unsigned int i = 0; i < 6; i++)
289301
{
290302
rb[3*i] = new RigidBody();
291303
rb[3*i]->initBody(0.0f,
292-
Eigen::Vector3f(startX, 1.0f, 1.0f),
304+
Eigen::Vector3f(startX, startY, 1.0f),
293305
computeInertiaTensorBox(1.0f, 0.5f, 0.5f, 0.5f),
294306
Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));
295307

296308
// dynamic body
297309
rb[3*i+1] = new RigidBody();
298310
rb[3*i+1]->initBody(1.0f,
299-
Eigen::Vector3f(startX, 0.75f, 2.0f),
311+
Eigen::Vector3f(startX, startY-0.25f, 2.0f),
300312
computeInertiaTensorBox(1.0f, width, height, depth),
301313
Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));
302314

303315
// dynamic body
304316
rb[3*i+2] = new RigidBody();
305317
rb[3*i+2]->initBody(1.0f,
306-
Eigen::Vector3f(startX, 0.75f, 4.0f),
318+
Eigen::Vector3f(startX, startY-0.25f, 4.0f),
307319
computeInertiaTensorBox(1.0f, width, height, depth),
308320
Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));
309321

310322
startX += 4.0f;
311-
}
312323

324+
if (i == 3)
325+
{
326+
startY -= 5.5f;
327+
startX = 0.0f;
328+
}
329+
}
313330

314-
model.addBallJoint(0, 1, Eigen::Vector3f(0.25f, 0.75f, 1.0f));
315-
model.addBallJoint(1, 2, Eigen::Vector3f(0.25f, 0.75f, 3.0f));
331+
float jointY = 0.75f;
332+
model.addBallJoint(0, 1, Eigen::Vector3f(0.25f, jointY, 1.0f));
333+
model.addBallJoint(1, 2, Eigen::Vector3f(0.25f, jointY, 3.0f));
316334

317-
model.addBallOnLineJoint(3, 4, Eigen::Vector3f(4.25f, 0.75f, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
318-
model.addBallJoint(4, 5, Eigen::Vector3f(4.25f, 0.75f, 3.0f));
335+
model.addBallOnLineJoint(3, 4, Eigen::Vector3f(4.25f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
336+
model.addBallJoint(4, 5, Eigen::Vector3f(4.25f, jointY, 3.0f));
319337

320-
model.addHingeJoint(6, 7, Eigen::Vector3f(8.0f, 0.75f, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
321-
model.addBallJoint(7, 8, Eigen::Vector3f(8.25f, 0.75f, 3.0f));
338+
model.addHingeJoint(6, 7, Eigen::Vector3f(8.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
339+
model.addBallJoint(7, 8, Eigen::Vector3f(8.25f, jointY, 3.0f));
322340

323-
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));
324-
model.addBallJoint(10, 11, Eigen::Vector3f(12.25f, 0.75f, 3.0f));
341+
model.addUniversalJoint(9, 10, Eigen::Vector3f(12.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
342+
model.addBallJoint(10, 11, Eigen::Vector3f(12.25f, jointY, 3.0f));
325343

326-
model.addTargetAngleMotorHingeJoint(12, 13, Eigen::Vector3f(16.0f, 0.75f, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
344+
jointY -= 5.5f;
345+
model.addTargetAngleMotorHingeJoint(12, 13, Eigen::Vector3f(0.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
327346
((TargetAngleMotorHingeJoint*)model.getConstraints()[model.getConstraints().size() - 1])->setMaxAngularMomentumPerStep(0.5f);
328-
model.addTargetVelocityMotorHingeJoint(13, 14, Eigen::Vector3f(16.0f, 0.75f, 3.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
347+
model.addTargetVelocityMotorHingeJoint(13, 14, Eigen::Vector3f(0.0f, jointY, 3.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
329348
((TargetVelocityMotorHingeJoint*)model.getConstraints()[model.getConstraints().size() - 1])->setMaxAngularMomentumPerStep(25.0f);
349+
350+
model.addSliderJoint(15, 16, Eigen::Vector3f(4.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
351+
model.addBallJoint(16, 17, Eigen::Vector3f(4.25f, jointY, 3.0f));
330352
}
331353

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

‎Demos/Simulation/Constraints.cpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ int StrainTetConstraint::TYPE_ID = 13;
2323
int ShapeMatchingConstraint::TYPE_ID = 14;
2424
int TargetAngleMotorHingeJoint::TYPE_ID = 15;
2525
int TargetVelocityMotorHingeJoint::TYPE_ID = 16;
26+
int SliderJoint::TYPE_ID = 17;
2627

2728
//////////////////////////////////////////////////////////////////////////
2829
// BallJoint
@@ -334,6 +335,84 @@ bool UniversalJoint::solvePositionConstraint(SimulationModel &model)
334335
}
335336

336337

338+
//////////////////////////////////////////////////////////////////////////
339+
// SliderJoint
340+
//////////////////////////////////////////////////////////////////////////
341+
bool SliderJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
342+
{
343+
m_bodies[0] = rbIndex1;
344+
m_bodies[1] = rbIndex2;
345+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
346+
RigidBody &rb1 = *rb[m_bodies[0]];
347+
RigidBody &rb2 = *rb[m_bodies[1]];
348+
return PositionBasedRigidBodyDynamics::init_SliderJoint(
349+
rb1.getPosition(),
350+
rb1.getRotation(),
351+
rb2.getPosition(),
352+
rb2.getRotation(),
353+
pos, axis,
354+
m_jointInfo);
355+
}
356+
357+
bool SliderJoint::updateConstraint(SimulationModel &model)
358+
{
359+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
360+
RigidBody &rb1 = *rb[m_bodies[0]];
361+
RigidBody &rb2 = *rb[m_bodies[1]];
362+
return PositionBasedRigidBodyDynamics::update_SliderJoint(
363+
rb1.getPosition(),
364+
rb1.getRotation(),
365+
rb2.getPosition(),
366+
rb2.getRotation(),
367+
m_jointInfo);
368+
}
369+
370+
bool SliderJoint::solvePositionConstraint(SimulationModel &model)
371+
{
372+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
373+
374+
RigidBody &rb1 = *rb[m_bodies[0]];
375+
RigidBody &rb2 = *rb[m_bodies[1]];
376+
377+
Eigen::Vector3f corr_x1, corr_x2;
378+
Eigen::Quaternionf corr_q1, corr_q2;
379+
const bool res = PositionBasedRigidBodyDynamics::solve_SliderJoint(
380+
rb1.getInvMass(),
381+
rb1.getPosition(),
382+
rb1.getInertiaTensorInverseW(),
383+
rb1.getRotation(),
384+
rb2.getInvMass(),
385+
rb2.getPosition(),
386+
rb2.getInertiaTensorInverseW(),
387+
rb2.getRotation(),
388+
m_jointInfo,
389+
corr_x1,
390+
corr_q1,
391+
corr_x2,
392+
corr_q2);
393+
394+
if (res)
395+
{
396+
if (rb1.getMass() != 0.0f)
397+
{
398+
rb1.getPosition() += corr_x1;
399+
rb1.getRotation().coeffs() += corr_q1.coeffs();
400+
rb1.getRotation().normalize();
401+
rb1.rotationUpdated();
402+
}
403+
if (rb2.getMass() != 0.0f)
404+
{
405+
rb2.getPosition() += corr_x2;
406+
rb2.getRotation().coeffs() += corr_q2.coeffs();
407+
rb2.getRotation().normalize();
408+
rb2.rotationUpdated();
409+
}
410+
}
411+
return res;
412+
}
413+
414+
415+
337416
//////////////////////////////////////////////////////////////////////////
338417
// TargetAngleMotorHingeJoint
339418
//////////////////////////////////////////////////////////////////////////

‎Demos/Simulation/Constraints.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,20 @@ namespace PBD
8585
virtual bool solvePositionConstraint(SimulationModel &model);
8686
};
8787

88+
class SliderJoint : public Constraint
89+
{
90+
public:
91+
static int TYPE_ID;
92+
Eigen::Matrix<float, 3, 12> m_jointInfo;
93+
94+
SliderJoint() : Constraint(2) {}
95+
virtual int &getTypeId() const { return TYPE_ID; }
96+
97+
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
98+
virtual bool updateConstraint(SimulationModel &model);
99+
virtual bool solvePositionConstraint(SimulationModel &model);
100+
};
101+
88102
class TargetAngleMotorHingeJoint : public Constraint
89103
{
90104
public:

‎Demos/Simulation/SimulationModel.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -148,6 +148,18 @@ bool SimulationModel::addUniversalJoint(const unsigned int rbIndex1, const unsig
148148
return res;
149149
}
150150

151+
bool SimulationModel::addSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
152+
{
153+
SliderJoint *joint = new SliderJoint();
154+
const bool res = joint->initConstraint(*this, rbIndex1, rbIndex2, pos, axis);
155+
if (res)
156+
{
157+
m_constraints.push_back(joint);
158+
m_groupsInitialized = false;
159+
}
160+
return res;
161+
}
162+
151163
bool SimulationModel::addTargetAngleMotorHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
152164
{
153165
TargetAngleMotorHingeJoint *hj = new TargetAngleMotorHingeJoint();

‎Demos/Simulation/SimulationModel.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ namespace PBD
8686
bool addTargetAngleMotorHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
8787
bool addTargetVelocityMotorHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
8888
bool addUniversalJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis1, const Eigen::Vector3f &axis2);
89+
bool addSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
8990
bool addRigidBodyParticleBallJoint(const unsigned int rbIndex, const unsigned int particleIndex);
9091

9192
bool addDistanceConstraint(const unsigned int particle1, const unsigned int particle2);

0 commit comments

Comments
 (0)