rbdlsim/include/rbdlsim.h

157 lines
3.8 KiB
C
Raw Normal View History

2020-10-03 22:55:14 +02:00
#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;
2020-11-20 14:17:15 +01:00
const double cCollisionEps = 1.0e-5;
2020-10-03 22:55:14 +02:00
struct SimShape {
enum ShapeType { Box = 0, Sphere = 1, Plane = 2 };
2020-10-03 22:55:14 +02:00
ShapeType mType;
Vector3d pos;
Quaternion orientation;
Vector3d scale;
2020-11-05 23:12:05 +01:00
double restitution = 1.0;
2020-10-03 22:55:14 +02:00
};
struct SimBody {
VectorNd q, qdot, qddot, tau;
Model mModel;
bool mIsStatic = false;
2020-10-03 22:55:14 +02:00
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);
2020-10-03 22:55:14 +02:00
void updateCollisionShapes();
};
struct CollisionInfo {
2020-11-05 23:12:05 +01:00
SimBody* mBodyA = nullptr;
SimBody* mBodyB = nullptr;
const SimShape* mShapeA = nullptr;
const SimShape* mShapeB = nullptr;
int mBodyAIndex;
int mBodyBIndex;
Vector3d mManifoldPoints[8];
2020-11-20 14:17:15 +01:00
int mNumManifoldPoints = 0;
Vector3d posA = Vector3d::Zero();
Vector3d posB = Vector3d::Zero();
double biasVelocityA = 0.;
double biasVelocityB = 0.;
double accumImpulse = 0.;
double deltaImpulse = 0.;
Vector3d dir = Vector3d::Zero();
VectorNd jacA = VectorNd::Zero(1);
VectorNd jacB = VectorNd::Zero(1);
VectorNd MInvJacTA = VectorNd::Zero(1);
VectorNd MInvJacTB = VectorNd::Zero(1);
double GMInvGTA = 0.;
double GMInvGTB = 0.;
double tangentAccumImpulse0 = 0.;
double tangentAccumImpulse1 = 0.;
double tangentDeltaImpulse0 = 0.;
double tangentDeltaImpulse1 = 0.;
Vector3d tangent0 = Vector3d::Zero();
Vector3d tangent1 = Vector3d::Zero();
VectorNd tangentJacA = MatrixNd::Zero(2,2);
VectorNd tangentJacB = MatrixNd::Zero(2,2);
MatrixNd tangentMInvA = MatrixNd::Zero(2, 2);
MatrixNd tangentMInvB = MatrixNd::Zero(2, 2);
MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2);
MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2);
2020-11-05 23:12:05 +01:00
double effectiveRestitution = 1.0;
double depth = 0.;
2020-10-03 22:55:14 +02:00
};
struct World {
double mSimTime = 0.;
std::vector<SimBody> mBodies;
std::vector<SimShape> mStaticShapes;
std::vector<CollisionInfo> mContactPoints;
2020-11-05 23:12:05 +01:00
void calcUnconstrainedVelUpdate(double dt);
2020-10-03 22:55:14 +02:00
void updateCollisionShapes();
void detectCollisions();
void resolveCollisions(double dt);
bool integrateWorld(double dt);
2020-10-03 22:55:14 +02:00
};
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);
2020-11-20 14:17:15 +01:00
bool CheckPenetrationBoxVsPlane (
const SimShape& shape_a,
const SimShape& shape_b,
CollisionInfo& cinfo);
bool CheckPenetrationAABBVsPlane(
const SimShape& shape_a,
const SimShape& shape_b,
CollisionInfo& cinfo);
2020-10-03 22:55:14 +02:00
SimBody CreateSphereBody(
double mass,
double radius,
2020-11-08 12:49:43 +01:00
double restitution,
2020-10-03 22:55:14 +02:00
const Vector3d& pos,
const Vector3d& vel);
SimBody CreateBoxBody(
double mass,
const Vector3d& size,
double restitution,
2020-10-03 22:55:14 +02:00
const Vector3d& pos,
const Vector3d& vel);
void PrepareConstraintImpulse(
double dt,
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo);
void CalcCollisions(
2020-11-08 12:49:43 +01:00
SimBody& body_a,
SimBody& body_b,
std::vector<CollisionInfo>& collisions);
void CalcConstraintImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo,
const double dt);
void ApplyConstraintImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo);
2020-10-03 22:55:14 +02:00
} // namespace RBDLSim
#endif //RBDLSIM_RBDLSIM_H