Skip to content

Commit e7d9420

Browse files
janbenderjanbender
committed
- added motor slider joint
1 parent 402cb57 commit e7d9420

File tree

11 files changed

+723
-139
lines changed

11 files changed

+723
-139
lines changed

‎Changelog.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
- added motor slider joint
12
- added slider joint
23
- extended JointDemo
34

‎Demos/CMakeLists.txt‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,14 @@
11
include(Visualization/CMakeLists.txt)
22

3-
subdirs(BarDemo ClothDemo FluidDemo RigidBodyDemos CouplingDemos GenericConstraintsDemos)
3+
subdirs(BarDemo ClothDemo FluidDemo RigidBodyDemos CouplingDemos GenericConstraintsDemos)
4+
5+
# A macro for copying the resources of a sample project
6+
MACRO(COPY_RESOURCES demoName)
7+
IF (WIN32)
8+
ADD_CUSTOM_COMMAND(
9+
TARGET ${demoName}
10+
POST_BUILD
11+
COMMAND xcopy /r /y \"${CMAKE_CURRENT_SOURCE_DIR}/Resources_${demoName}\" \"$\(TargetDir\)..\\..\\bin\\$\(ConfigurationName\)\\Resources_${demoName}\\\"
12+
)
13+
ENDIF (WIN32)
14+
ENDMACRO(COPY_RESOURCES)

‎Demos/RigidBodyDemos/JointDemo.cpp‎

Lines changed: 26 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -145,6 +145,10 @@ void timeStep ()
145145
TargetVelocityMotorHingeJoint &joint2 = (*(TargetVelocityMotorHingeJoint*)constraints[9]);
146146
joint1.setTargetAngle(currentTargetAngle);
147147
joint2.setTargetAngularVelocity(3.5f);
148+
149+
const float currentTargetPos = 1.5f*sin(2.0f*TimeManager::getCurrent()->getTime());
150+
TargetPositionMotorSliderJoint &joint3 = (*(TargetPositionMotorSliderJoint*)constraints[12]);
151+
joint3.setTargetPosition(currentTargetPos);
148152
}
149153
}
150154

@@ -185,7 +189,14 @@ void renderUniversalJoint(UniversalJoint &uj)
185189

186190
void renderSliderJoint(SliderJoint &joint)
187191
{
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);
192+
MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
193+
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);
194+
}
195+
196+
void renderTargetPositionMotorSliderJoint(TargetPositionMotorSliderJoint &joint)
197+
{
198+
MiniGL::drawSphere(joint.m_jointInfo.col(6), 0.1f, jointColor);
199+
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);
189200
}
190201

191202
void renderTargetAngleMotorHingeJoint(TargetAngleMotorHingeJoint &hj)
@@ -263,6 +274,10 @@ void render ()
263274
{
264275
renderTargetVelocityMotorHingeJoint(*(TargetVelocityMotorHingeJoint*)constraints[i]);
265276
}
277+
else if (constraints[i]->getTypeId() == TargetPositionMotorSliderJoint::TYPE_ID)
278+
{
279+
renderTargetPositionMotorSliderJoint(*(TargetPositionMotorSliderJoint*)constraints[i]);
280+
}
266281
}
267282

268283
float textColor[4] = { 0.0f, .2f, .4f, 1 };
@@ -273,6 +288,7 @@ void render ()
273288

274289
MiniGL::drawStrokeText(-1.0f, -4.0f, 1.0f, 0.002f, "motor hinge joint", 17, textColor);
275290
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);
276292

277293
MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
278294
}
@@ -294,10 +310,10 @@ void createBodyModel()
294310
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
295311

296312
// static body
297-
rb.resize(18);
313+
rb.resize(21);
298314
float startX = 0.0f;
299315
float startY = 1.0f;
300-
for (unsigned int i = 0; i < 6; i++)
316+
for (unsigned int i = 0; i < 7; i++)
301317
{
302318
rb[3*i] = new RigidBody();
303319
rb[3*i]->initBody(0.0f,
@@ -313,9 +329,9 @@ void createBodyModel()
313329
Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));
314330

315331
// dynamic body
316-
rb[3*i+2] = new RigidBody();
317-
rb[3*i+2]->initBody(1.0f,
318-
Eigen::Vector3f(startX, startY-0.25f, 4.0f),
332+
rb[3 * i + 2] = new RigidBody();
333+
rb[3 * i + 2]->initBody(1.0f,
334+
Eigen::Vector3f(startX, startY - 0.25f, 4.0f),
319335
computeInertiaTensorBox(1.0f, width, height, depth),
320336
Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f));
321337

@@ -343,12 +359,13 @@ void createBodyModel()
343359

344360
jointY -= 5.5f;
345361
model.addTargetAngleMotorHingeJoint(12, 13, Eigen::Vector3f(0.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
346-
((TargetAngleMotorHingeJoint*)model.getConstraints()[model.getConstraints().size() - 1])->setMaxAngularMomentumPerStep(0.5f);
347362
model.addTargetVelocityMotorHingeJoint(13, 14, Eigen::Vector3f(0.0f, jointY, 3.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
348-
((TargetVelocityMotorHingeJoint*)model.getConstraints()[model.getConstraints().size() - 1])->setMaxAngularMomentumPerStep(25.0f);
349-
363+
350364
model.addSliderJoint(15, 16, Eigen::Vector3f(4.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
351365
model.addBallJoint(16, 17, Eigen::Vector3f(4.25f, jointY, 3.0f));
366+
367+
model.addTargetPositionMotorSliderJoint(18, 19, Eigen::Vector3f(8.0f, jointY, 1.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
368+
model.addBallJoint(19, 20, Eigen::Vector3f(8.25f, jointY, 3.0f));
352369
}
353370

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

‎Demos/Simulation/Constraints.cpp‎

Lines changed: 78 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ int ShapeMatchingConstraint::TYPE_ID = 14;
2424
int TargetAngleMotorHingeJoint::TYPE_ID = 15;
2525
int TargetVelocityMotorHingeJoint::TYPE_ID = 16;
2626
int SliderJoint::TYPE_ID = 17;
27+
int TargetPositionMotorSliderJoint::TYPE_ID = 18;
2728

2829
//////////////////////////////////////////////////////////////////////////
2930
// BallJoint
@@ -412,6 +413,83 @@ bool SliderJoint::solvePositionConstraint(SimulationModel &model)
412413
}
413414

414415

416+
//////////////////////////////////////////////////////////////////////////
417+
// TargetPositionMotorSliderJoint
418+
//////////////////////////////////////////////////////////////////////////
419+
bool TargetPositionMotorSliderJoint::initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
420+
{
421+
m_bodies[0] = rbIndex1;
422+
m_bodies[1] = rbIndex2;
423+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
424+
RigidBody &rb1 = *rb[m_bodies[0]];
425+
RigidBody &rb2 = *rb[m_bodies[1]];
426+
return PositionBasedRigidBodyDynamics::init_TargetPositionMotorSliderJoint(
427+
rb1.getPosition(),
428+
rb1.getRotation(),
429+
rb2.getPosition(),
430+
rb2.getRotation(),
431+
pos, axis,
432+
m_jointInfo);
433+
}
434+
435+
bool TargetPositionMotorSliderJoint::updateConstraint(SimulationModel &model)
436+
{
437+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
438+
RigidBody &rb1 = *rb[m_bodies[0]];
439+
RigidBody &rb2 = *rb[m_bodies[1]];
440+
return PositionBasedRigidBodyDynamics::update_TargetPositionMotorSliderJoint(
441+
rb1.getPosition(),
442+
rb1.getRotation(),
443+
rb2.getPosition(),
444+
rb2.getRotation(),
445+
m_jointInfo);
446+
}
447+
448+
bool TargetPositionMotorSliderJoint::solvePositionConstraint(SimulationModel &model)
449+
{
450+
SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
451+
452+
RigidBody &rb1 = *rb[m_bodies[0]];
453+
RigidBody &rb2 = *rb[m_bodies[1]];
454+
455+
Eigen::Vector3f corr_x1, corr_x2;
456+
Eigen::Quaternionf corr_q1, corr_q2;
457+
const bool res = PositionBasedRigidBodyDynamics::solve_TargetPositionMotorSliderJoint(
458+
rb1.getInvMass(),
459+
rb1.getPosition(),
460+
rb1.getInertiaTensorInverseW(),
461+
rb1.getRotation(),
462+
rb2.getInvMass(),
463+
rb2.getPosition(),
464+
rb2.getInertiaTensorInverseW(),
465+
rb2.getRotation(),
466+
m_targetPosition,
467+
m_jointInfo,
468+
corr_x1,
469+
corr_q1,
470+
corr_x2,
471+
corr_q2);
472+
473+
if (res)
474+
{
475+
if (rb1.getMass() != 0.0f)
476+
{
477+
rb1.getPosition() += corr_x1;
478+
rb1.getRotation().coeffs() += corr_q1.coeffs();
479+
rb1.getRotation().normalize();
480+
rb1.rotationUpdated();
481+
}
482+
if (rb2.getMass() != 0.0f)
483+
{
484+
rb2.getPosition() += corr_x2;
485+
rb2.getRotation().coeffs() += corr_q2.coeffs();
486+
rb2.getRotation().normalize();
487+
rb2.rotationUpdated();
488+
}
489+
}
490+
return res;
491+
}
492+
415493

416494
//////////////////////////////////////////////////////////////////////////
417495
// TargetAngleMotorHingeJoint
@@ -464,7 +542,6 @@ bool TargetAngleMotorHingeJoint::solvePositionConstraint(SimulationModel &model)
464542
rb2.getInertiaTensorInverseW(),
465543
rb2.getRotation(),
466544
m_targetAngle,
467-
m_maxAngularMomentumPerStep,
468545
m_jointInfo,
469546
corr_x1,
470547
corr_q1,
@@ -588,7 +665,6 @@ bool TargetVelocityMotorHingeJoint::solveVelocityConstraint(SimulationModel &mod
588665
rb2.getInertiaTensorInverseW(),
589666
rb2.getAngularVelocity(),
590667
m_targetAngularVelocity,
591-
m_maxAngularMomentumPerStep,
592668
m_jointInfo,
593669
corr_v1,
594670
corr_omega1,

‎Demos/Simulation/Constraints.h‎

Lines changed: 21 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ namespace PBD
8989
{
9090
public:
9191
static int TYPE_ID;
92-
Eigen::Matrix<float, 3, 12> m_jointInfo;
92+
Eigen::Matrix<float, 3, 14> m_jointInfo;
9393

9494
SliderJoint() : Constraint(2) {}
9595
virtual int &getTypeId() const { return TYPE_ID; }
@@ -99,18 +99,33 @@ namespace PBD
9999
virtual bool solvePositionConstraint(SimulationModel &model);
100100
};
101101

102+
class TargetPositionMotorSliderJoint : public Constraint
103+
{
104+
public:
105+
static int TYPE_ID;
106+
Eigen::Matrix<float, 3, 14> m_jointInfo;
107+
float m_targetPosition;
108+
109+
TargetPositionMotorSliderJoint() : Constraint(2) { m_targetPosition = 0.0f; }
110+
virtual int &getTypeId() const { return TYPE_ID; }
111+
112+
float getTargetPosition() const { return m_targetPosition; }
113+
void setTargetPosition(const float val) { m_targetPosition = val; }
114+
115+
bool initConstraint(SimulationModel &model, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
116+
virtual bool updateConstraint(SimulationModel &model);
117+
virtual bool solvePositionConstraint(SimulationModel &model);
118+
};
119+
102120
class TargetAngleMotorHingeJoint : public Constraint
103121
{
104122
public:
105123
static int TYPE_ID;
106124
Eigen::Matrix<float, 3, 14> m_jointInfo;
107125
float m_targetAngle;
108-
float m_maxAngularMomentumPerStep;
109-
TargetAngleMotorHingeJoint() : Constraint(2) { m_targetAngle = 0.0f; m_maxAngularMomentumPerStep = 0.0f; }
126+
TargetAngleMotorHingeJoint() : Constraint(2) { m_targetAngle = 0.0f; }
110127
virtual int &getTypeId() const { return TYPE_ID; }
111128

112-
float getMaxAngularMomentumPerStep() const { return m_maxAngularMomentumPerStep; }
113-
void setMaxAngularMomentumPerStep(const float val) { m_maxAngularMomentumPerStep = val; }
114129
float getTargetAngle() const { return m_targetAngle; }
115130
void setTargetAngle(const float val)
116131
{
@@ -130,12 +145,9 @@ namespace PBD
130145
static int TYPE_ID;
131146
Eigen::Matrix<float, 3, 14> m_jointInfo;
132147
float m_targetAngularVelocity;
133-
float m_maxAngularMomentumPerStep;
134-
TargetVelocityMotorHingeJoint() : Constraint(2) { m_targetAngularVelocity = 0.0f; m_maxAngularMomentumPerStep = 0.0f; }
148+
TargetVelocityMotorHingeJoint() : Constraint(2) { m_targetAngularVelocity = 0.0f; }
135149
virtual int &getTypeId() const { return TYPE_ID; }
136150

137-
float getMaxAngularMomentumPerStep() const { return m_maxAngularMomentumPerStep; }
138-
void setMaxAngularMomentumPerStep(const float val) { m_maxAngularMomentumPerStep = val; }
139151
float getTargetAngularVelocity() const { return m_targetAngularVelocity; }
140152
void setTargetAngularVelocity(const float val) { m_targetAngularVelocity = val; }
141153

‎Demos/Simulation/SimulationModel.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,18 @@ bool SimulationModel::addSliderJoint(const unsigned int rbIndex1, const unsigned
160160
return res;
161161
}
162162

163+
bool SimulationModel::addTargetPositionMotorSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
164+
{
165+
TargetPositionMotorSliderJoint *joint = new TargetPositionMotorSliderJoint();
166+
const bool res = joint->initConstraint(*this, rbIndex1, rbIndex2, pos, axis);
167+
if (res)
168+
{
169+
m_constraints.push_back(joint);
170+
m_groupsInitialized = false;
171+
}
172+
return res;
173+
}
174+
163175
bool SimulationModel::addTargetAngleMotorHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
164176
{
165177
TargetAngleMotorHingeJoint *hj = new TargetAngleMotorHingeJoint();

‎Demos/Simulation/SimulationModel.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,7 @@ namespace PBD
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);
8989
bool addSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
90+
bool addTargetPositionMotorSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis);
9091
bool addRigidBodyParticleBallJoint(const unsigned int rbIndex, const unsigned int particleIndex);
9192

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

0 commit comments

Comments
 (0)