martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Sort of working bouncing simulation.

master
Martin Felis 2020-11-05 23:12:05 +01:00
parent d1fa076581
commit 8ef9a923d8
4 changed files with 83 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/rbdlsim.h
View File

@ -23,6 +23,7 @@ struct SimShape {
Vector3d pos;
Quaternion orientation;
Vector3d scale;
double restitution = 1.0;
};
struct SimBody {
@ -40,16 +41,18 @@ struct SimBody {
};
struct CollisionInfo {
const SimBody* mBodyA = nullptr;
const SimBody* mBodyB = nullptr;
SimBody* mBodyA = nullptr;
SimBody* mBodyB = nullptr;
const SimShape* mShapeA = nullptr;
const SimShape* mShapeB = nullptr;
int mBodyAIndex;
int mBodyBIndex;
Vector3d posA = Vector3d::Zero();
Vector3d posB = Vector3d::Zero();
Vector3d accumImpulseA = Vector3d::Zero();
Vector3d accumImpulseB = Vector3d::Zero();
double accumImpulseA = 0.;
double accumImpulseB = 0.;
double deltaImpulseA = 0.;
double deltaImpulseB = 0.;
Vector3d dir = Vector3d::Zero();
VectorNd jacA = VectorNd::Zero(1);
VectorNd jacB = VectorNd::Zero(1);
@ -57,6 +60,7 @@ struct CollisionInfo {
MatrixNd MInvB = MatrixNd::Zero(1, 1);
double GMInvGTA = 0.;
double GMInvGTB = 0.;
double effectiveRestitution = 1.0;
double depth = 0.;
};
@ -67,6 +71,7 @@ struct World {
std::vector<SimShape> mStaticShapes;
std::vector<CollisionInfo> mContactPoints;
void calcUnconstrainedVelUpdate(double dt);
void updateCollisionShapes();
void detectCollisions();
void resolveCollisions(double dt);

119
src/rbdlsim.cc
View File

@ -1,4 +1,5 @@
#include "rbdlsim.h"
#include "utils.h"
#include <ccd/ccd.h>
#include <ccd/quat.h>
@ -255,8 +256,8 @@ bool SolveGaussSeidelProj(
}
void CalcCollisions(
const SimBody& body_a,
const SimBody& body_b,
SimBody& body_a,
SimBody& body_b,
std::vector<CollisionInfo>& collisions) {
collisions.clear();
@ -269,6 +270,7 @@ void CalcCollisions(
CollisionInfo cinfo;
bool has_penetration = false;
has_penetration = CheckPenetration(shape_a, shape_b, cinfo);
cinfo.effectiveRestitution = shape_a.restitution * shape_b.restitution;
if (has_penetration) {
cinfo.mBodyA = &body_a;
@ -289,7 +291,7 @@ void CalcImpulseVariables(
MatrixNd* MInv,
VectorNd* jac,
double* G_MInv_GT) {
if (body->mIsStatic) {
if (body == nullptr || body->mIsStatic) {
*G_MInv_GT = 0.;
return;
}
@ -346,21 +348,30 @@ void CalcConstraintImpulse(
// Todo: add nonlinear effects * dt
double rhs = 0.;
if (!body_a->mIsStatic) {
rhs += cinfo.jacA * body_a->qdot;
if (body_a && !body_a->mIsStatic) {
rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot;
}
if (!body_b->mIsStatic) {
rhs += cinfo.jacB * body_b->qdot;
if (body_b && !body_b->mIsStatic) {
rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
}
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
if (!body_a->mIsStatic) {
cinfo.accumImpulseA = -rhs / denom * cinfo.dir;
if (body_a && !body_a->mIsStatic) {
double old_impulse = cinfo.accumImpulseA;
cinfo.deltaImpulseA = -rhs / denom;
cinfo.accumImpulseA += cinfo.deltaImpulseA;
cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA);
cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse;
}
if (!body_b->mIsStatic) {
cinfo.accumImpulseB = rhs / denom * cinfo.dir;
if (body_b && !body_b->mIsStatic) {
double old_impulse = cinfo.accumImpulseB;
cinfo.deltaImpulseB = std::max(0., rhs / denom);
cinfo.accumImpulseB += cinfo.deltaImpulseB;
cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
gLog ("deltaImpulse: %f", cinfo.deltaImpulseB);
}
}
@ -374,14 +385,14 @@ void ApplyConstraintImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo) {
if (!body_a->mIsStatic) {
if (body_a && !body_a->mIsStatic) {
body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
* (-cinfo.dir.transpose() * cinfo.accumImpulseB);
* (-cinfo.deltaImpulseA);
}
if (!body_b->mIsStatic) {
if (body_b && !body_b->mIsStatic) {
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
* (-cinfo.dir.transpose() * cinfo.accumImpulseB);
* (-cinfo.deltaImpulseB);
}
}
@ -471,6 +482,15 @@ void SimBody::resolveCollisions(
qdot = qdot + Minv * (dt * tau + dt * N + G.transpose() * hlambda);
}
void World::calcUnconstrainedVelUpdate(double dt) {
for (SimBody& body : mBodies) {
ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
// semi-implicit eulers
body.qdot += dt * body.qddot;
}
}
void World::updateCollisionShapes() {
for (SimBody& body : mBodies) {
body.updateCollisionShapes();
@ -480,21 +500,32 @@ void World::updateCollisionShapes() {
void World::detectCollisions() {
mContactPoints.clear();
for (SimBody& body : mBodies) {
for (SimBody::BodyCollisionInfo& body_col_info : body.mCollisionShapes) {
SimShape& body_shape = body_col_info.second;
for (int i = 0, n = mBodies.size(); i < n; ++i) {
SimBody& ref_body = mBodies[i];
// Check collisions against moving bodies
for (int j = i + 1; j < n; ++j) {
SimBody& other_body = mBodies[j];
CalcCollisions(ref_body, other_body, mContactPoints);
}
// Check collisions against static bodies
for (SimBody::BodyCollisionInfo& body_col_info :
ref_body.mCollisionShapes) {
SimShape& ref_body_shape = body_col_info.second;
// check against all static shapes
for (SimShape& static_shape : mStaticShapes) {
bool has_penetration = false;
CollisionInfo cinfo;
has_penetration = CheckPenetration(static_shape, body_shape, cinfo);
has_penetration = CheckPenetration(static_shape, ref_body_shape, cinfo);
cinfo.effectiveRestitution = ref_body_shape.restitution;
if (has_penetration) {
cinfo.mBodyA = nullptr;
cinfo.mBodyAIndex = -1;
cinfo.mBodyB = &body;
cinfo.mBodyB = &ref_body;
cinfo.mBodyBIndex = body_col_info.first;
mContactPoints.push_back(cinfo);
}
@ -504,46 +535,15 @@ void World::detectCollisions() {
}
void World::resolveCollisions(double dt) {
// so far only solve Body vs World collisions
for (SimBody& body : mBodies) {
// collect all collisions for current body
std::vector<CollisionInfo> body_collisions;
for (CollisionInfo& cinfo : mContactPoints) {
PrepareConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
}
for (const CollisionInfo cinfo : mContactPoints) {
if (cinfo.mBodyA == &body) {
body_collisions.push_back(cinfo);
} else if (cinfo.mBodyB == &body) {
// Make sure the collision info is expressed in terms
// of mBodyA.
CollisionInfo rev_cinfo;
rev_cinfo.mBodyA = &body;
rev_cinfo.mShapeA = cinfo.mShapeB;
rev_cinfo.mBodyAIndex = cinfo.mBodyBIndex;
rev_cinfo.mBodyB = cinfo.mBodyA;
rev_cinfo.mShapeB = cinfo.mShapeA;
rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex;
Vector3d temp = cinfo.posA;
rev_cinfo.posA = cinfo.posB;
rev_cinfo.posA = temp;
rev_cinfo.dir = -cinfo.dir;
rev_cinfo.depth = cinfo.depth;
body_collisions.push_back(rev_cinfo);
}
}
if (body_collisions.size() > 0) {
cout << "Collision at t = " << mSimTime
<< ", pos = " << body_collisions[0].posA.transpose()
<< ", depth = " << body_collisions[0].depth << endl
<< " qdotpre = " << mBodies[0].qdot.transpose() << endl;
}
body.resolveCollisions(dt, body_collisions);
if (body_collisions.size() > 0) {
cout << " qdotpost = " << mBodies[0].qdot.transpose() << endl;
int num_iter = 20;
for (int i = 0; i < num_iter; i++) {
for (CollisionInfo& cinfo : mContactPoints) {
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
}
}
}
@ -596,6 +596,7 @@ SimBody CreateSphereBody(
shape.pos = pos;
shape.orientation.set(0., 0., 0., 1.);
shape.scale.set(radius, radius, radius);
shape.restitution = 0.3;
result.mCollisionShapes.push_back(
SimBody::BodyCollisionInfo(sphere_body, shape));

13
src/simulator.cc
View File

@ -17,6 +17,7 @@ using namespace RBDLSim;
static World sWorld;
static SimShape sGroundShape;
static SimBody sSphereBody;
static SimBody sSphereBody2;
typedef SimpleMath::Matrix<float, 3, 3> Matrix33f;
typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
@ -35,9 +36,13 @@ void simulator_init() {
sWorld.mStaticShapes.push_back(sGroundShape);
sSphereBody =
CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
CreateSphereBody(1., 1., Vector3d(0., 1.405, 0.), Vector3d::Zero());
sWorld.mBodies.push_back(sSphereBody);
sSphereBody2 =
CreateSphereBody(1., 1., Vector3d(0.3, 2.405, 0.), Vector3d::Zero());
// sWorld.mBodies.push_back(sSphereBody2);
sWorld.mSimTime = 0.;
}
@ -65,6 +70,12 @@ void simulator_update(double dt) {
}
ImGui::End();
sWorld.calcUnconstrainedVelUpdate(dt);
sWorld.updateCollisionShapes();
sWorld.detectCollisions();
sWorld.resolveCollisions(dt);
sWorld.integrateWorld(dt);
}
void simulator_draw(srcmdbuf* cmdbuf) {

4
src/vissim.cc
View File

@ -166,7 +166,7 @@ void DoRender() {
simd4x4f view;
simd4x4f proj;
simd4x4f_translation(&view, 0.1f, 0.1f, -0.5f);
simd4f eye = simd4f_create (2.f * sin(gTimer->mCurrentTime), 2 *sin(gTimer->mCurrentTime * 0.5f), 2.f * cos (gTimer->mCurrentTime), 1.f);
simd4f eye = simd4f_create (0.f, 1.f, 10.f, 1.f);
simd4x4f_lookat(
&view,
eye,
@ -185,7 +185,7 @@ void DoRender() {
-50.0f,
50.0f);
simd4x4f_perspective(&proj,
110.f * (M_PI / 180.f), view_width / view_height, 0.1f, 50.f);
70.f * (M_PI / 180.f), view_width / view_height, 0.1f, 50.f);
srview_set_view(gView, view);
srview_set_proj(gView, proj);