76 lines
1.4 KiB
C++
76 lines
1.4 KiB
C++
#ifndef RBDLSIM_RBDLSIM_H
|
|
#define RBDLSIM_RBDLSIM_H
|
|
|
|
#include <rbdl/Model.h>
|
|
#include <rbdl/rbdl_math.h>
|
|
|
|
#include <vector>
|
|
|
|
namespace RBDLSim {
|
|
|
|
using namespace RigidBodyDynamics;
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
struct SimShape {
|
|
enum ShapeType {
|
|
Box = 0,
|
|
Sphere = 1,
|
|
};
|
|
ShapeType mType;
|
|
Vector3d pos;
|
|
Quaternion orientation;
|
|
Vector3d scale;
|
|
};
|
|
|
|
struct SimBody {
|
|
VectorNd q, qdot, qddot, tau;
|
|
Model mModel;
|
|
|
|
typedef std::pair<unsigned int, SimShape> BodyCollisionInfo;
|
|
std::vector<BodyCollisionInfo> mCollisionShapes;
|
|
|
|
void step(double ts);
|
|
void updateCollisionShapes();
|
|
};
|
|
|
|
struct CollisionInfo {
|
|
const SimBody* mBodyA;
|
|
const SimBody* mBodyB;
|
|
const SimShape* mShapeA;
|
|
const SimShape* mShapeB;
|
|
Vector3d pos;
|
|
Vector3d dir;
|
|
};
|
|
|
|
struct World {
|
|
double mSimTime = 0.;
|
|
std::vector<SimBody> mBodies;
|
|
std::vector<SimShape> mStaticShapes;
|
|
std::vector<CollisionInfo> mContactPoints;
|
|
|
|
void updateCollisionShapes();
|
|
void detectCollisions();
|
|
bool step(double dt);
|
|
};
|
|
|
|
bool CheckPenetration(
|
|
const SimShape& shape_a,
|
|
const SimShape& shape_b,
|
|
CollisionInfo& cinfo);
|
|
|
|
SimBody CreateSphereBody(
|
|
double mass,
|
|
double radius,
|
|
const Vector3d& pos,
|
|
const Vector3d& vel);
|
|
|
|
SimBody CreateBoxBody(
|
|
double mass,
|
|
const Vector3d& size,
|
|
const Vector3d& pos,
|
|
const Vector3d& vel);
|
|
|
|
} // namespace RBDLSim
|
|
|
|
#endif //RBDLSIM_RBDLSIM_H
|