Skip to content

Commit 5e111cc

Browse files
janbenderjanbender
committed
- added rigid body demo
- added balljoint constraint
1 parent f4fb2cf commit 5e111cc

14 files changed

+1113
-3
lines changed

‎Changelog.txt

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,10 @@
1+
1.2.0
2+
- added rigid body demo
3+
- added balljoint constraint
4+
- fixed WIN32 issues
5+
- cleanup
6+
- Ubuntu fix
7+
18
1.1.0
29

310
- added second order velocity update

‎Demos/CMakeLists.txt

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

3-
subdirs(BarDemo ClothDemo FluidDemo)
3+
subdirs(BarDemo ClothDemo FluidDemo RigidBodyDemo)

‎Demos/ClothDemo/TriangleModel.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,8 @@ namespace PBD
3232
virtual ~TriangleModel();
3333

3434
typedef IndexedFaceMesh<ParticleData> ParticleMesh;
35-
typedef std::vector<BendingConstraint, Eigen::aligned_allocator<BendingConstraint>> BendingConstraintVector;
36-
typedef std::vector<TriangleConstraint, Eigen::aligned_allocator<TriangleConstraint>> TriangleConstraintVector;
35+
typedef std::vector<BendingConstraint, Eigen::aligned_allocator<BendingConstraint> > BendingConstraintVector;
36+
typedef std::vector<TriangleConstraint, Eigen::aligned_allocator<TriangleConstraint> > TriangleConstraintVector;
3737

3838
protected:
3939
/** Face mesh of particles which represents the simulation model */

‎Demos/RigidBodyDemo/CMakeLists.txt

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
set(SIMULATION_LINK_LIBRARIES AntTweakBar glew PositionBasedDynamics)
2+
set(SIMULATION_DEPENDENCIES AntTweakBar glew PositionBasedDynamics)
3+
4+
if(WIN32)
5+
set(SIMULATION_LINK_LIBRARIES freeglut opengl32.lib glu32.lib ${SIMULATION_LINK_LIBRARIES})
6+
set(SIMULATION_DEPENDENCIES freeglut ${SIMULATION_DEPENDENCIES})
7+
else()
8+
find_package(GLUT REQUIRED)
9+
find_package(OpenGL REQUIRED)
10+
11+
set(SIMULATION_LINK_LIBRARIES
12+
${SIMULATION_LINK_LIBRARIES}
13+
${GLUT_LIBRARIES}
14+
${OPENGL_LIBRARIES}
15+
)
16+
endif()
17+
18+
add_executable(RigidBodyDemo
19+
main.cpp
20+
21+
TimeStepRigidBodyModel.cpp
22+
TimeStepRigidBodyModel.h
23+
RigidBodyModel.cpp
24+
RigidBodyModel.h
25+
26+
${VIS_FILES}
27+
${PROJECT_PATH}/Demos/Utils/Config.h
28+
${PROJECT_PATH}/Demos/Utils/TimeManager.cpp
29+
${PROJECT_PATH}/Demos/Utils/TimeManager.h
30+
${PROJECT_PATH}/Demos/Utils/RigidBody.h
31+
32+
CMakeLists.txt
33+
)
34+
35+
add_definitions(-DTW_NO_LIB_PRAGMA -DTW_STATIC)
36+
37+
find_package( Eigen3 REQUIRED )
38+
include_directories( ${EIGEN3_INCLUDE_DIR} )
39+
include_directories(${PROJECT_PATH}/extern/freeglut/include)
40+
include_directories(${PROJECT_PATH}/extern/glew/include)
41+
42+
add_dependencies(RigidBodyDemo ${SIMULATION_DEPENDENCIES})
43+
target_link_libraries(RigidBodyDemo ${SIMULATION_LINK_LIBRARIES})
44+
VIS_SOURCE_GROUPS()

‎Demos/RigidBodyDemo/RigidBodyModel.cpp

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include "RigidBodyModel.h"
2+
#include "PositionBasedDynamics/PositionBasedDynamics.h"
3+
4+
using namespace PBD;
5+
6+
RigidBodyModel::RigidBodyModel()
7+
{
8+
}
9+
10+
RigidBodyModel::~RigidBodyModel(void)
11+
{
12+
m_rigidBodies.clear();
13+
m_ballJoints.clear();
14+
}
15+
16+
void RigidBodyModel::reset()
17+
{
18+
for (size_t i = 0; i < m_rigidBodies.size(); i++)
19+
{
20+
m_rigidBodies[i].getPosition() = m_rigidBodies[i].getPosition0();
21+
m_rigidBodies[i].getOldPosition() = m_rigidBodies[i].getPosition0();
22+
m_rigidBodies[i].getLastPosition() = m_rigidBodies[i].getPosition0();
23+
24+
m_rigidBodies[i].getRotation() = m_rigidBodies[i].getRotation0();
25+
m_rigidBodies[i].getOldRotation() = m_rigidBodies[i].getRotation0();
26+
m_rigidBodies[i].getLastRotation() = m_rigidBodies[i].getRotation0();
27+
28+
m_rigidBodies[i].getVelocity().setZero();
29+
m_rigidBodies[i].getAngularVelocity().setZero();
30+
31+
m_rigidBodies[i].getAcceleration().setZero();
32+
m_rigidBodies[i].getTorque().setZero();
33+
34+
m_rigidBodies[i].rotationUpdated();
35+
}
36+
updateBallJoints();
37+
}
38+
39+
RigidBodyModel::RigidBodyVector & RigidBodyModel::getRigidBodies()
40+
{
41+
return m_rigidBodies;
42+
}
43+
44+
RigidBodyModel::BallJointVector & RigidBodyModel::getBallJoints()
45+
{
46+
return m_ballJoints;
47+
}
48+
49+
50+
void RigidBodyModel::setBallJoint(const unsigned int i, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos)
51+
{
52+
m_ballJoints[i].m_index[0] = rbIndex1;
53+
m_ballJoints[i].m_index[1] = rbIndex2;
54+
55+
m_ballJoints[i].m_points[0] = pos;
56+
m_ballJoints[i].m_points[1] = pos;
57+
58+
// transform in local coordinates
59+
RigidBody &r1 = m_rigidBodies[rbIndex1];
60+
RigidBody &r2 = m_rigidBodies[rbIndex2];
61+
62+
const Eigen::Matrix3f rot1 = r1.getRotation0().matrix();
63+
const Eigen::Matrix3f rot2 = r2.getRotation0().matrix();
64+
65+
m_ballJoints[i].m_localPoints[0] = rot1 * (pos - r1.getPosition0());
66+
m_ballJoints[i].m_localPoints[1] = rot2 * (pos - r2.getPosition0());
67+
}
68+
69+
void RigidBodyModel::updateBallJoints()
70+
{
71+
for (unsigned int i = 0; i < m_ballJoints.size(); i++)
72+
{
73+
updateBallJoint(i);
74+
}
75+
}
76+
77+
void RigidBodyModel::updateBallJoint(const unsigned int i)
78+
{
79+
BallJoint &bj = m_ballJoints[i];
80+
for (unsigned int j = 0; j < 2; j++)
81+
{
82+
RigidBody &rb = m_rigidBodies[bj.m_index[j]];
83+
bj.m_points[j] = rb.getRotationMatrix() * bj.m_localPoints[j] + rb.getPosition();
84+
}
85+
}

‎Demos/RigidBodyDemo/RigidBodyModel.h

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
#ifndef __RIGIDBODYMODEL_H__
2+
#define __RIGIDBODYMODEL_H__
3+
4+
#include "Demos/Utils/Config.h"
5+
#include <vector>
6+
#include "Demos/Utils/RigidBody.h"
7+
8+
namespace PBD
9+
{
10+
class RigidBodyModel
11+
{
12+
public:
13+
struct BallJoint
14+
{
15+
/** indices of the linked rigid bodies */
16+
unsigned int m_index[2];
17+
/** joint points in world space */
18+
Eigen::Vector3f m_points[2];
19+
/** joint points in local coordinates */
20+
Eigen::Vector3f m_localPoints[2];
21+
};
22+
23+
RigidBodyModel();
24+
virtual ~RigidBodyModel();
25+
26+
typedef std::vector<BallJoint, Eigen::aligned_allocator<BallJoint> > BallJointVector;
27+
typedef std::vector<RigidBody, Eigen::aligned_allocator<RigidBody> > RigidBodyVector;
28+
29+
protected:
30+
RigidBodyVector m_rigidBodies;
31+
BallJointVector m_ballJoints;
32+
33+
public:
34+
virtual void reset();
35+
36+
RigidBodyVector &getRigidBodies();
37+
BallJointVector &getBallJoints();
38+
39+
void setBallJoint(const unsigned int i, const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos);
40+
void updateBallJoints();
41+
void updateBallJoint(const unsigned int i);
42+
};
43+
}
44+
45+
#endif

‎Demos/RigidBodyDemo/TimeStepRigidBodyModel.cpp

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
#include "TimeStepRigidBodyModel.h"
2+
#include "Demos/Utils/TimeManager.h"
3+
#include "PositionBasedDynamics/PositionBasedDynamics.h"
4+
#include "PositionBasedDynamics/TimeIntegration.h"
5+
6+
using namespace PBD;
7+
using namespace std;
8+
9+
TimeStepRigidBodyModel::TimeStepRigidBodyModel()
10+
{
11+
m_velocityUpdateMethod = 0;
12+
}
13+
14+
TimeStepRigidBodyModel::~TimeStepRigidBodyModel(void)
15+
{
16+
}
17+
18+
void TimeStepRigidBodyModel::step(RigidBodyModel &model)
19+
{
20+
TimeManager *tm = TimeManager::getCurrent ();
21+
const float h = tm->getTimeStepSize();
22+
23+
clearAccelerations(model);
24+
RigidBodyModel::RigidBodyVector &rb = model.getRigidBodies();
25+
for (size_t i = 0; i < rb.size(); i++)
26+
{
27+
rb[i].getLastPosition() = rb[i].getOldPosition();
28+
rb[i].getOldPosition() = rb[i].getPosition();
29+
TimeIntegration::semiImplicitEuler(h, rb[i].getMass(), rb[i].getPosition(), rb[i].getVelocity(), rb[i].getAcceleration());
30+
rb[i].getLastRotation() = rb[i].getOldRotation();
31+
rb[i].getOldRotation() = rb[i].getRotation();
32+
TimeIntegration::semiImplicitEulerRotation(h, rb[i].getMass(), rb[i].getInertiaTensorInverseW(), rb[i].getRotation(), rb[i].getAngularVelocity(), rb[i].getTorque());
33+
rb[i].rotationUpdated();
34+
}
35+
36+
constraintProjection(model);
37+
38+
// Update velocities
39+
for (size_t i = 0; i < rb.size(); i++)
40+
{
41+
if (m_velocityUpdateMethod == 0)
42+
{
43+
TimeIntegration::velocityUpdateFirstOrder(h, rb[i].getMass(), rb[i].getPosition(), rb[i].getOldPosition(), rb[i].getVelocity());
44+
TimeIntegration::angularVelocityUpdateFirstOrder(h, rb[i].getMass(), rb[i].getRotation(), rb[i].getOldRotation(), rb[i].getAngularVelocity());
45+
}
46+
else
47+
{
48+
TimeIntegration::velocityUpdateSecondOrder(h, rb[i].getMass(), rb[i].getPosition(), rb[i].getOldPosition(), rb[i].getLastPosition(), rb[i].getVelocity());
49+
TimeIntegration::angularVelocityUpdateSecondOrder(h, rb[i].getMass(), rb[i].getRotation(), rb[i].getOldRotation(), rb[i].getLastRotation(), rb[i].getAngularVelocity());
50+
}
51+
}
52+
53+
// compute new time
54+
tm->setTime (tm->getTime () + h);
55+
}
56+
57+
/** Clear accelerations and add gravitation.
58+
*/
59+
void TimeStepRigidBodyModel::clearAccelerations(RigidBodyModel &model)
60+
{
61+
RigidBodyModel::RigidBodyVector &rb = model.getRigidBodies();
62+
const Eigen::Vector3f grav(0.0f, -9.81f, 0.0f);
63+
for (size_t i=0; i < rb.size(); i++)
64+
{
65+
// Clear accelerations of dynamic particles
66+
if (rb[i].getMass() != 0.0)
67+
{
68+
Eigen::Vector3f &a = rb[i].getAcceleration();
69+
a = grav;
70+
}
71+
}
72+
}
73+
74+
void TimeStepRigidBodyModel::reset(RigidBodyModel &model)
75+
{
76+
77+
}
78+
79+
void TimeStepRigidBodyModel::constraintProjection(RigidBodyModel &model)
80+
{
81+
const unsigned int maxIter = 5;
82+
unsigned int iter = 0;
83+
84+
RigidBodyModel::RigidBodyVector &rb = model.getRigidBodies();
85+
RigidBodyModel::BallJointVector &bj = model.getBallJoints();
86+
87+
while (iter < maxIter)
88+
{
89+
for (unsigned int i = 0; i < bj.size(); i++)
90+
{
91+
model.updateBallJoint(i);
92+
93+
RigidBody &rb1 = rb[bj[i].m_index[0]];
94+
RigidBody &rb2 = rb[bj[i].m_index[1]];
95+
96+
Eigen::Vector3f corr_x1, corr_x2;
97+
Eigen::Quaternionf corr_q1, corr_q2;
98+
const bool res = PositionBasedDynamics::solveRigidBodyBallJoint(
99+
bj[i].m_points[0],
100+
rb1.getMass(),
101+
rb1.getPosition(),
102+
rb1.getInertiaTensorInverseW(),
103+
rb1.getRotation(),
104+
bj[i].m_points[1],
105+
rb2.getMass(),
106+
rb2.getPosition(),
107+
rb2.getInertiaTensorInverseW(),
108+
rb2.getRotation(),
109+
corr_x1,
110+
corr_q1,
111+
corr_x2,
112+
corr_q2);
113+
114+
if (res)
115+
{
116+
if (rb1.getMass() != 0.0f)
117+
{
118+
rb1.getPosition() += corr_x1;
119+
rb1.getRotation().coeffs() += corr_q1.coeffs();
120+
rb1.getRotation().normalize();
121+
rb1.rotationUpdated();
122+
}
123+
if (rb2.getMass() != 0.0f)
124+
{
125+
rb2.getPosition() += corr_x2;
126+
rb2.getRotation().coeffs() += corr_q2.coeffs();
127+
rb2.getRotation().normalize();
128+
rb2.rotationUpdated();
129+
}
130+
}
131+
}
132+
iter++;
133+
}
134+
}

‎Demos/RigidBodyDemo/TimeStepRigidBodyModel.h

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef __TimeStepRigidBodyModel_h__
2+
#define __TimeStepRigidBodyModel_h__
3+
4+
#include "Demos/Utils/Config.h"
5+
#include "RigidBodyModel.h"
6+
7+
namespace PBD
8+
{
9+
class TimeStepRigidBodyModel
10+
{
11+
protected:
12+
unsigned int m_velocityUpdateMethod;
13+
14+
void clearAccelerations(RigidBodyModel &model);
15+
void constraintProjection(RigidBodyModel &model);
16+
17+
public:
18+
TimeStepRigidBodyModel();
19+
virtual ~TimeStepRigidBodyModel(void);
20+
21+
void step(RigidBodyModel &model);
22+
void reset(RigidBodyModel &model);
23+
24+
unsigned int getVelocityUpdateMethod() const { return m_velocityUpdateMethod; }
25+
void setVelocityUpdateMethod(unsigned int val) { m_velocityUpdateMethod = val; }
26+
};
27+
}
28+
29+
#endif

0 commit comments

Comments
 (0)