Skip to content

Commit 7c3e413

Browse files
janbenderjanbender
committed
- update to Eigen 3.2.7
- added target velocity motor slider joint
1 parent 730e2fb commit 7c3e413

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

64 files changed

+979
-226
lines changed

‎Changelog.txt‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
1-
- added motor slider joint
1+
- update to Eigen 3.2.7
2+
- added target velocity motor slider joint
3+
- added target position motor slider joint
24
- added slider joint
35
- extended JointDemo
46

‎Demos/RigidBodyDemos/JointDemo.cpp‎

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,12 @@ void timeStep ()
149149
const float currentTargetPos = 1.5f*sin(2.0f*TimeManager::getCurrent()->getTime());
150150
TargetPositionMotorSliderJoint &joint3 = (*(TargetPositionMotorSliderJoint*)constraints[12]);
151151
joint3.setTargetPosition(currentTargetPos);
152+
153+
float currentTargetVel = 0.25f;
154+
if (((int) (0.25f*TimeManager::getCurrent()->getTime())) % 2 == 1)
155+
currentTargetVel = -currentTargetVel;
156+
TargetVelocityMotorSliderJoint &joint4 = (*(TargetVelocityMotorSliderJoint*)constraints[14]);
157+
joint4.setTargetVelocity(currentTargetVel);
152158
}
153159
}
154160

@@ -199,6 +205,12 @@ void renderTargetPositionMotorSliderJoint(TargetPositionMotorSliderJoint &joint)
199205
MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(8), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(8), jointColor, 0.05f);
200206
}
201207

208+
void renderTargetVelocityMotorSliderJoint(TargetVelocityMotorSliderJoint &joint)
209+
{
210+
MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
211+
MiniGL::drawCylinder(joint.m_jointInfo.col(7) - joint.m_jointInfo.col(8), joint.m_jointInfo.col(7) + joint.m_jointInfo.col(8), jointColor, 0.05f);
212+
}
213+
202214
void renderTargetAngleMotorHingeJoint(TargetAngleMotorHingeJoint &hj)
203215
{
204216
MiniGL::drawSphere(hj.m_jointInfo.col(6) - 0.5*hj.m_jointInfo.col(8), 0.1f, jointColor);
@@ -278,6 +290,10 @@ void render ()
278290
{
279291
renderTargetPositionMotorSliderJoint(*(TargetPositionMotorSliderJoint*)constraints[i]);
280292
}
293+
else if (constraints[i]->getTypeId() == TargetVelocityMotorSliderJoint::TYPE_ID)
294+
{
295+
renderTargetVelocityMotorSliderJoint(*(TargetVelocityMotorSliderJoint*)constraints[i]);
296+
}
281297
}
282298

283299
float textColor[4] = { 0.0f, .2f, .4f, 1 };
@@ -288,7 +304,8 @@ void render ()
288304

289305
MiniGL::drawStrokeText(-1.0f, -4.0f, 1.0f, 0.002f, "motor hinge joint", 17, textColor);
290306
MiniGL::drawStrokeText(3.4f, -4.0f, 1.0f, 0.002f, "slider joint", 12, textColor);
291-
MiniGL::drawStrokeText(7.0f, -4.0f, 1.0f, 0.002f, "motor slider joint", 18, textColor);
307+
MiniGL::drawStrokeText(6.6f, -4.0f, 1.0f, 0.002f, "target position motor", 21, textColor);
308+
MiniGL::drawStrokeText(10.6f, -4.0f, 1.0f, 0.002f, "target velocity motor", 21, textColor);
292309

293310
MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
294311
}
@@ -310,10 +327,10 @@ void createBodyModel()
310327
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
311328

312329
// static body
313-
rb.resize(21);
330+
rb.resize(24);
314331
float startX = 0.0f;
315332
float startY = 1.0f;
316-
for (unsigned int i = 0; i < 7; i++)
333+
for (unsigned int i = 0; i < 8; i++)
317334
{
318335
rb[3*i] = new RigidBody();
319336
rb[3*i]->initBody(0.0f,
@@ -366,6 +383,9 @@ void createBodyModel()
366383

367384
model.addTargetPositionMotorSliderJoint(18, 19, Eigen::Vector3f(8.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
368385
model.addBallJoint(19, 20, Eigen::Vector3f(8.25f, jointY, 3.0f));
386+
387+
model.addTargetVelocityMotorSliderJoint(21, 22, Eigen::Vector3f(12.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
388+
model.addBallJoint(22, 23, Eigen::Vector3f(12.25f, jointY, 3.0f));
369389
}
370390

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

‎Demos/Simulation/Constraints.cpp‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ int TargetAngleMotorHingeJoint::TYPE_ID = 15;
2525
int TargetVelocityMotorHingeJoint::TYPE_ID = 16;
2626
int SliderJoint::TYPE_ID = 17;
2727
int TargetPositionMotorSliderJoint::TYPE_ID = 18;
28+
int TargetVelocityMotorSliderJoint::TYPE_ID = 19;
2829

2930
//////////////////////////////////////////////////////////////////////////
3031
// BallJoint
@@ -491,6 +492,127 @@ bool TargetPositionMotorSliderJoint::solvePositionConstraint(SimulationModel &mo
491492
}
492493

493494

495+
496+
//////////////////////////////////////////////////////////////////////////
497+
// TargetVelocityMotorSliderJoint
498+
//////////////////////////////////////////////////////////////////////////
499+
bool TargetVelocityMotorSliderJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
500+
{
501+
m_bodies[0] = rbIndex1;
502+
m_bodies[1] = rbIndex2;
503+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
504+
RigidBody &rb1 = *rb[m_bodies[0]];
505+
RigidBody &rb2 = *rb[m_bodies[1]];
506+
return PositionBasedRigidBodyDynamics::init_TargetVelocityMotorSliderJoint(
507+
rb1.getPosition(),
508+
rb1.getRotation(),
509+
rb2.getPosition(),
510+
rb2.getRotation(),
511+
pos, axis,
512+
m_jointInfo);
513+
}
514+
515+
bool TargetVelocityMotorSliderJoint::updateConstraint(SimulationModel &model)
516+
{
517+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
518+
RigidBody &rb1 = *rb[m_bodies[0]];
519+
RigidBody &rb2 = *rb[m_bodies[1]];
520+
return PositionBasedRigidBodyDynamics::update_TargetVelocityMotorSliderJoint(
521+
rb1.getPosition(),
522+
rb1.getRotation(),
523+
rb2.getPosition(),
524+
rb2.getRotation(),
525+
m_jointInfo);
526+
}
527+
528+
bool TargetVelocityMotorSliderJoint::solvePositionConstraint(SimulationModel &model)
529+
{
530+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
531+
532+
RigidBody &rb1 = *rb[m_bodies[0]];
533+
RigidBody &rb2 = *rb[m_bodies[1]];
534+
535+
Eigen::Vector3f corr_x1, corr_x2;
536+
Eigen::Quaternionf corr_q1, corr_q2;
537+
const bool res = PositionBasedRigidBodyDynamics::solve_TargetVelocityMotorSliderJoint(
538+
rb1.getInvMass(),
539+
rb1.getPosition(),
540+
rb1.getInertiaTensorInverseW(),
541+
rb1.getRotation(),
542+
rb2.getInvMass(),
543+
rb2.getPosition(),
544+
rb2.getInertiaTensorInverseW(),
545+
rb2.getRotation(),
546+
m_jointInfo,
547+
corr_x1,
548+
corr_q1,
549+
corr_x2,
550+
corr_q2);
551+
552+
if (res)
553+
{
554+
if (rb1.getMass() != 0.0f)
555+
{
556+
rb1.getPosition() += corr_x1;
557+
rb1.getRotation().coeffs() += corr_q1.coeffs();
558+
rb1.getRotation().normalize();
559+
rb1.rotationUpdated();
560+
}
561+
if (rb2.getMass() != 0.0f)
562+
{
563+
rb2.getPosition() += corr_x2;
564+
rb2.getRotation().coeffs() += corr_q2.coeffs();
565+
rb2.getRotation().normalize();
566+
rb2.rotationUpdated();
567+
}
568+
}
569+
return res;
570+
}
571+
572+
573+
bool TargetVelocityMotorSliderJoint::solveVelocityConstraint(SimulationModel &model)
574+
{
575+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
576+
577+
RigidBody &rb1 = *rb[m_bodies[0]];
578+
RigidBody &rb2 = *rb[m_bodies[1]];
579+
580+
Eigen::Vector3f corr_v1, corr_v2;
581+
Eigen::Vector3f corr_omega1, corr_omega2;
582+
const bool res = PositionBasedRigidBodyDynamics::velocitySolve_TargetVelocityMotorSliderJoint(
583+
rb1.getInvMass(),
584+
rb1.getPosition(),
585+
rb1.getVelocity(),
586+
rb1.getInertiaTensorInverseW(),
587+
rb1.getAngularVelocity(),
588+
rb2.getInvMass(),
589+
rb2.getPosition(),
590+
rb2.getVelocity(),
591+
rb2.getInertiaTensorInverseW(),
592+
rb2.getAngularVelocity(),
593+
m_targetVelocity,
594+
m_jointInfo,
595+
corr_v1,
596+
corr_omega1,
597+
corr_v2,
598+
corr_omega2);
599+
600+
if (res)
601+
{
602+
if (rb1.getMass() != 0.0f)
603+
{
604+
rb1.getVelocity() += corr_v1;
605+
rb1.getAngularVelocity() += corr_omega1;
606+
}
607+
if (rb2.getMass() != 0.0f)
608+
{
609+
rb2.getVelocity() += corr_v2;
610+
rb2.getAngularVelocity() += corr_omega2;
611+
}
612+
}
613+
return res;
614+
}
615+
494616
//////////////////////////////////////////////////////////////////////////
495617
// TargetAngleMotorHingeJoint
496618
//////////////////////////////////////////////////////////////////////////

‎Demos/Simulation/Constraints.h‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,25 @@ namespace PBD
117117
virtual bool solvePositionConstraint(SimulationModel &model);
118118
};
119119

120+
class TargetVelocityMotorSliderJoint : public Constraint
121+
{
122+
public:
123+
static int TYPE_ID;
124+
Eigen::Matrix<float, 3, 14> m_jointInfo;
125+
float m_targetVelocity;
126+
127+
TargetVelocityMotorSliderJoint() : Constraint(2) { m_targetVelocity = 0.0f; }
128+
virtual int &getTypeId() const { return TYPE_ID; }
129+
130+
float getTargetVelocity() const { return m_targetVelocity; }
131+
void setTargetVelocity(const float val) { m_targetVelocity = val; }
132+
133+
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
134+
virtual bool updateConstraint(SimulationModel &model);
135+
virtual bool solvePositionConstraint(SimulationModel &model);
136+
virtual bool solveVelocityConstraint(SimulationModel &model);
137+
};
138+
120139
class TargetAngleMotorHingeJoint : public Constraint
121140
{
122141
public:

‎Demos/Simulation/SimulationModel.cpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,19 @@ bool SimulationModel::addTargetPositionMotorSliderJoint(const unsigned int rbInd
175175
return res;
176176
}
177177

178+
bool SimulationModel::addTargetVelocityMotorSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
179+
{
180+
TargetVelocityMotorSliderJoint *joint = new TargetVelocityMotorSliderJoint();
181+
const bool res = joint->initConstraint(*this, rbIndex1, rbIndex2, pos, axis);
182+
if (res)
183+
{
184+
m_constraints.push_back(joint);
185+
m_groupsInitialized = false;
186+
}
187+
return res;
188+
}
189+
190+
178191
bool SimulationModel::addTargetAngleMotorHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
179192
{
180193
TargetAngleMotorHingeJoint *hj = new TargetAngleMotorHingeJoint();

‎Demos/Simulation/SimulationModel.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ namespace PBD
8888
bool addUniversalJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis1, const Eigen::Vector3f &axis2);
8989
bool addSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
9090
bool addTargetPositionMotorSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
91+
bool addTargetVelocityMotorSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
9192
bool addRigidBodyParticleBallJoint(const unsigned int rbIndex, const unsigned int particleIndex);
9293

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

0 commit comments

Comments
 (0)