96 lines
2.0 KiB
C++
96 lines
2.0 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;
|
|
struct SimBody;
|
|
struct CollisionInfo;
|
|
|
|
const double cCollisionEps = 1.0e-4;
|
|
|
|
struct SimShape {
|
|
enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
|
|
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 resolveCollisions(double dt, std::vector<CollisionInfo>& collisions);
|
|
void calcNextPositions(double dt, const VectorNd& in_qdot, VectorNd& out_q);
|
|
void updateCollisionShapes();
|
|
};
|
|
|
|
struct CollisionInfo {
|
|
const SimBody* mBodyA;
|
|
const SimBody* mBodyB;
|
|
const SimShape* mShapeA;
|
|
const SimShape* mShapeB;
|
|
int mBodyAIndex;
|
|
int mBodyBIndex;
|
|
Vector3d posA;
|
|
Vector3d posB;
|
|
Vector3d dir;
|
|
double depth;
|
|
};
|
|
|
|
struct World {
|
|
double mSimTime = 0.;
|
|
std::vector<SimBody> mBodies;
|
|
std::vector<SimShape> mStaticShapes;
|
|
std::vector<CollisionInfo> mContactPoints;
|
|
|
|
void updateCollisionShapes();
|
|
void detectCollisions();
|
|
void resolveCollisions(double dt);
|
|
bool integrateWorld(double dt);
|
|
};
|
|
|
|
bool CheckPenetration(
|
|
const SimShape& shape_a,
|
|
const SimShape& shape_b,
|
|
CollisionInfo& cinfo);
|
|
|
|
bool CheckPenetrationSphereVsSphere(
|
|
const SimShape& shape_a,
|
|
const SimShape& shape_b,
|
|
CollisionInfo& cinfo);
|
|
|
|
bool CheckPenetrationSphereVsPlane(
|
|
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
|