https://github.com/xbpeng/DeepTerrainRL
Tip revision: ed82e2ebe5f14fa875cc3d0a2180c64980408e8f authored by Glen on 19 October 2016, 17:49:36 UTC
Update README.md
Update README.md
Tip revision: ed82e2e
World.cpp
#include "World.h"
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "sim/SimObj.h"
#include "sim/SimBox.h"
#include "sim/SimCapsule.h"
#include "sim/SimPlane.h"
#include "sim/Joint.h"
cWorld::tParams::tParams()
{
mNumSubsteps = 1;
mScale = 1;
mGravity = gGravity;
}
cWorld::tJointParams::tJointParams()
{
mType = eJointTypeHinge;
mAnchor0 = tVector::Zero();
mAnchor1 = tVector::Zero();
mAnchorAxis1 = tVector(0, 0, 1, 0);
mAnchorTheta1 = 0;
mAxis = tVector(0, 1, 0, 0);
mEnableAdjacentCollision = true;
mLimLow = 1; // low > high -> no limit
mLimHigh = 0;
mRefTheta = 0;
}
cWorld::tConstraintHandle::tConstraintHandle()
{
mCons = nullptr;
}
bool cWorld::tConstraintHandle::IsValid() const
{
return mCons != nullptr;
}
void cWorld::tConstraintHandle::Clear()
{
delete mCons;
mCons = nullptr;
}
cWorld::cWorld()
: mContactManager(*this)
{
mLinearDamping = 0;
mAngularDamping = 0;
}
cWorld::~cWorld()
{
ClearConstraints();
}
void cWorld::Init(const tParams& params)
{
mParams = params;
mBroadPhase = std::unique_ptr<btBroadphaseInterface>(new btDbvtBroadphase());
mCollisionConfig = std::unique_ptr<btDefaultCollisionConfiguration>(new btDefaultCollisionConfiguration());
mCollisionDispatcher = std::unique_ptr<btCollisionDispatcher>(new btCollisionDispatcher(mCollisionConfig.get()));
btGImpactCollisionAlgorithm::registerAlgorithm(mCollisionDispatcher.get());
mSolver = std::unique_ptr<btSequentialImpulseConstraintSolver>(new btSequentialImpulseConstraintSolver());
mSimWorld = std::unique_ptr<btDiscreteDynamicsWorld>(new btDiscreteDynamicsWorld(mCollisionDispatcher.get(),
mBroadPhase.get(), mSolver.get(), mCollisionConfig.get()));
SetGravity(params.mGravity);
mContactManager.Init();
mPerturbManager.Clear();
}
void cWorld::Reset()
{
mContactManager.Reset();
mPerturbManager.Clear();
mSimWorld->clearForces();
mSolver->reset();
mBroadPhase->resetPool(mCollisionDispatcher.get());
btOverlappingPairCache* pair_cache = mSimWorld->getBroadphase()->getOverlappingPairCache();
btBroadphasePairArray& pair_array = pair_cache->getOverlappingPairArray();
for (int i = 0; i < pair_array.size(); ++i)
{
pair_cache->cleanOverlappingPair(pair_array[i], mSimWorld->getDispatcher());
}
}
void cWorld::Update(double time_elapsed)
{
time_elapsed = std::max(0.0, time_elapsed);
mPerturbManager.Update(time_elapsed);
btScalar time_step = static_cast<btScalar>(time_elapsed);
mSimWorld->stepSimulation(time_step, mParams.mNumSubsteps, time_step / mParams.mNumSubsteps);
mContactManager.Update();
}
void cWorld::AddObject(cSimObj& obj)
{
const std::unique_ptr<btRigidBody>& body = obj.GetRigidBody();
short col_group = obj.GetColGroup();
short col_mask = obj.GetColMask();
col_mask |= cContactManager::gFlagRayTest;
body->setDamping(static_cast<btScalar>(mLinearDamping), static_cast<btScalar>(mAngularDamping));
mSimWorld->addRigidBody(body.get(), col_group, col_mask);
}
void cWorld::RemoveObject(cSimObj& obj)
{
/*
if (obj.GetRigidBody() && obj.GetRigidBody()->getMotionState())
{
delete obj.GetRigidBody().get()->getMotionState();
}*/
// mSimWorld->removeCollisionObject(obj.GetRigidBody().get());
mSimWorld->removeRigidBody(obj.GetRigidBody().get());
// std::cout << "Number of colission objects" << mSimWorld->m_collisionShapes.size() << std::endl;
// obj.GetRigidBody().reset();
}
cWorld::tConstraintHandle cWorld::AddJoint(cSimObj* obj, const tJointParams& params)
{
return AddJoint(obj, nullptr, params);
}
cWorld::tConstraintHandle cWorld::AddJoint(cSimObj* obj0, cSimObj* obj1, const tJointParams& params)
{
tConstraintHandle handle;
switch (params.mType)
{
case eJointTypeHinge:
handle = AddHingeConstraint(obj0, obj1, params);
break;
case eJointTypePrismatic:
handle = AddPrismaticConstraint(obj0, obj1, params);
break;
default:
assert(false);
printf("Unsupported constraint type\n");
break;
}
return handle;
}
void cWorld::ConstrainPlane(cSimObj* obj, ePlaneCons plane_cons)
{
tVector linear_factor;
tVector angular_factor;
BuildConsFactor(plane_cons, linear_factor, angular_factor);
Constrain(obj, linear_factor, angular_factor);
}
void cWorld::Constrain(cSimObj* obj, const tVector& linear_factor, const tVector& angular_factor)
{
auto& body = obj->GetRigidBody();
body->setLinearFactor(btVector3(static_cast<btScalar>(linear_factor[0]),
static_cast<btScalar>(linear_factor[1]),
static_cast<btScalar>(linear_factor[2])));
body->setAngularFactor(btVector3(static_cast<btScalar>(angular_factor[0]),
static_cast<btScalar>(angular_factor[1]),
static_cast<btScalar>(angular_factor[2])));
}
void cWorld::RemoveConstraint(tConstraintHandle& handle)
{
if (handle.IsValid())
{
mSimWorld->removeConstraint(handle.mCons);
}
handle.Clear();
}
void cWorld::BuildConsFactor(ePlaneCons plane_cons, tVector& out_linear_factor, tVector& out_angular_factor)
{
out_linear_factor = tVector::Ones();
out_angular_factor = tVector::Ones();
switch (plane_cons)
{
case ePlaneConsNone:
break;
case ePlaneConsXY:
out_linear_factor = tVector(1, 1, 0, 0);
out_angular_factor = tVector(0, 0, 1, 0);
break;
default:
assert(false); // unsupported constraint
}
}
void cWorld::SetGravity(const tVector& gravity)
{
double scale = GetScale();
mSimWorld->setGravity(btVector3(static_cast<btScalar>(gravity[0] * scale),
static_cast<btScalar>(gravity[1] * scale),
static_cast<btScalar>(gravity[2] * scale)));
}
cContactManager::tContactHandle cWorld::RegisterContact(int contact_flags, int filter_flags)
{
return mContactManager.RegisterContact(contact_flags, filter_flags);
}
void cWorld::UpdateContact(const cContactManager::tContactHandle& handle)
{
mContactManager.UpdateContact(handle);
}
tVector cWorld::GetContactPt(const cContactManager::tContactHandle& handle) const
{
return mContactManager.GetContactPt(handle);
}
bool cWorld::IsInContact(const cContactManager::tContactHandle& handle) const
{
return mContactManager.IsInContact(handle);
}
void cWorld::RayTest(const tVector& beg, const tVector& end, tRayTestResults& results) const
{
btScalar scale = static_cast<btScalar>(GetScale());
btVector3 bt_beg = scale * btVector3(static_cast<btScalar>(beg[0]),
static_cast<btScalar>(beg[1]),
static_cast<btScalar>(beg[2]));
btVector3 bt_end = scale * btVector3(static_cast<btScalar>(end[0]),
static_cast<btScalar>(end[1]),
static_cast<btScalar>(end[2]));
btCollisionWorld::ClosestRayResultCallback ray_callback(bt_beg, bt_end);
mSimWorld->rayTest(bt_beg, bt_end, ray_callback);
results.clear();
if (ray_callback.hasHit())
{
auto& obj = ray_callback.m_collisionObject;
const auto& hit_pt = ray_callback.m_hitPointWorld;
tRayTestResult result;
result.mObj = static_cast<cSimObj*>(obj->getUserPointer());
result.mHitPos = tVector(hit_pt[0], hit_pt[1], hit_pt[2], 0) / scale;
results.push_back(result);
}
}
void cWorld::AddPerturb(const tPerturb& perturb)
{
mPerturbManager.AddPerturb(perturb);
}
const cPerturbManager& cWorld::GetPerturbManager() const
{
return mPerturbManager;
}
tVector cWorld::GetGravity() const
{
double scale = GetScale();
btVector3 bt_gravity = mSimWorld->getGravity();
return tVector(bt_gravity[0], bt_gravity[1], bt_gravity[2], 0) / scale;
}
double cWorld::GetScale() const
{
return mParams.mScale;
}
void cWorld::SetLinearDamping(double damping)
{
mLinearDamping = damping;
}
void cWorld::SetAngularDamping(double damping)
{
mAngularDamping = damping;
}
tVector cWorld::GetPos(const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
btTransform trans = body->getWorldTransform();
btVector3 origin = trans.getOrigin();
tVector pos = tVector(origin[0], origin[1], origin[2], 0);
pos /= GetScale();
return pos;
}
void cWorld::SetPos(const tVector& pos, cSimObj* out_obj) const
{
auto& body = out_obj->GetRigidBody();
btTransform trans = body->getWorldTransform();
btScalar scale = static_cast<btScalar>(GetScale());
trans.setOrigin(scale * btVector3(static_cast<btScalar>(pos[0]),
static_cast<btScalar>(pos[1]),
static_cast<btScalar>(pos[2])));
body->setWorldTransform(trans);
body->activate();
}
tVector cWorld::GetLinearVelocity(const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
const btVector3& bt_vel = body->getLinearVelocity();
tVector vel = tVector(bt_vel[0], bt_vel[1], bt_vel[2], 0);
vel /= GetScale();
return vel;
}
tVector cWorld::GetLinearVelocity(const cSimObj* obj, const tVector& local_pos) const
{
auto& body = obj->GetRigidBody();
btScalar scale = static_cast<btScalar>(GetScale());
tMatrix3 rot_mat = obj->GetLocalToWorldRotMat();
tVector rel_pos = tVector::Zero();
rel_pos.segment(0, 3) = rot_mat * local_pos.segment(0, 3);
btVector3 bt_vel = body->getVelocityInLocalPoint(scale * btVector3(static_cast<btScalar>(rel_pos[0]),
static_cast<btScalar>(rel_pos[1]),
static_cast<btScalar>(rel_pos[2])));
tVector vel = tVector(bt_vel[0], bt_vel[1], bt_vel[2], 0);
vel /= GetScale();
return vel;
}
void cWorld::SetLinearVelocity(const tVector& vel, cSimObj* out_obj) const
{
auto& body = out_obj->GetRigidBody();
btScalar scale = static_cast<btScalar>(GetScale());
body->setLinearVelocity(scale * btVector3(static_cast<btScalar>(vel[0]),
static_cast<btScalar>(vel[1]),
static_cast<btScalar>(vel[2])));
body->activate();
}
tVector cWorld::GetAngularVelocity(const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
const btVector3& vel = body->getAngularVelocity();
return tVector(vel[0], vel[1], vel[2], 0);
}
void cWorld::SetAngularVelocity(const tVector& vel, const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
body->setAngularVelocity(btVector3(static_cast<btScalar>(vel[0]), static_cast<btScalar>(vel[1]),
static_cast<btScalar>(vel[2])));
body->activate();
}
tMatrix cWorld::GetWorldTransform(const cSimObj* obj) const
{
tMatrix trans = GetWorldTransformAux(obj);
return trans;
}
tMatrix cWorld::GetLocalTransform(const cSimObj* obj) const
{
tMatrix trans = GetWorldTransformAux(obj);
trans = cMathUtil::InvRigidMat(trans);
return trans;
}
void cWorld::GetRotation(const cSimObj* obj, tVector& out_axis, double& out_theta) const
{
btQuaternion q = GetRotQuaternion(obj);
btVector3 axis = q.getAxis();
out_theta = q.getAngle();
out_axis[0] = axis[0];
out_axis[1] = axis[1];
out_axis[2] = axis[2];
out_axis[3] = 0;
}
btQuaternion cWorld::GetRotQuaternion(const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
btTransform trans = body->getWorldTransform();
btQuaternion q = trans.getRotation();
return q;
}
tMatrix cWorld::GetWorldTransformAux(const cSimObj* obj) const
{
auto& body = obj->GetRigidBody();
const btTransform& bt_trans = body->getWorldTransform();
const btMatrix3x3& basis = bt_trans.getBasis();
const btVector3& origin = bt_trans.getOrigin();
double scale = GetScale();
tMatrix trans = tMatrix::Identity();
for (int i = 0; i < 3; ++i)
{
auto curr_row = trans.row(i);
auto bt_row = basis.getRow(i);
for (int j = 0; j < 3; ++j)
{
curr_row[j] = bt_row[j];
}
curr_row[3] = origin[i] / scale;
}
return trans;
}
void cWorld::SetRotation(const tVector& axis, double theta, cSimObj* out_obj) const
{
auto& body = out_obj->GetRigidBody();
btTransform trans = body->getWorldTransform();
double norm = axis.norm();
btVector3 bt_axis = btVector3(static_cast<btScalar>(axis[0]), static_cast<btScalar>(axis[1]),
static_cast<btScalar>(axis[2]));
bt_axis.normalize();
btQuaternion q;
q.setRotation(bt_axis, static_cast<btScalar>(theta));
trans.setRotation(q);
body->setWorldTransform(trans);
}
void cWorld::CalcAABB(const cSimObj* obj, tVector& out_min, tVector& out_max) const
{
auto& body = obj->GetRigidBody();
btVector3 bt_min;
btVector3 bt_max;
body->getAabb(bt_min, bt_max);
double scale = GetScale();
out_min = tVector(bt_min[0], bt_min[1], bt_min[2], 0) / scale;
out_max = tVector(bt_max[0], bt_max[1], bt_max[2], 0) / scale;
}
void cWorld::ApplyForce(const tVector& force, const tVector& local_pos, cSimObj* out_obj) const
{
auto& body = out_obj->GetRigidBody();
btVector3 bt_force = btVector3(static_cast<btScalar>(force[0]),
static_cast<btScalar>(force[1]),
static_cast<btScalar>(force[2]));
btScalar scale = static_cast<btScalar>(GetScale());
tMatrix3 rot_mat = out_obj->GetLocalToWorldRotMat();
tVector rel_pos = tVector::Zero();
rel_pos.segment(0, 3) = rot_mat * local_pos.segment(0, 3);
btVector3 bt_pos = btVector3(static_cast<btScalar>(rel_pos[0]),
static_cast<btScalar>(rel_pos[1]),
static_cast<btScalar>(rel_pos[2]));
bt_force *= scale;
bt_pos *= scale;
body->applyForce(bt_force, bt_pos);
}
void cWorld::ApplyTorque(const tVector& torque, cSimObj* out_obj) const
{
auto& body = out_obj->GetRigidBody();
btScalar scale = static_cast<btScalar>(GetScale());
body->applyTorque(scale * scale * btVector3(static_cast<btScalar>(torque[0]),
static_cast<btScalar>(torque[1]),
static_cast<btScalar>(torque[2])));
}
std::unique_ptr<btBoxShape> cWorld::BuildBoxShape(const tVector& box_size) const
{
btScalar scale = static_cast<btScalar>(GetScale());
std::unique_ptr<btBoxShape> shape = std::unique_ptr<btBoxShape>(new btBoxShape(scale * btVector3(static_cast<btScalar>(box_size[0] * 0.5),
static_cast<btScalar>(box_size[1] * 0.5),
static_cast<btScalar>(box_size[2] * 0.5))));
return shape;
}
std::unique_ptr<btCapsuleShape> cWorld::BuildCapsuleShape(double radius, double height) const
{
btScalar scale = static_cast<btScalar>(GetScale());
std::unique_ptr<btCapsuleShape> shape = std::unique_ptr<btCapsuleShape>(new btCapsuleShape(static_cast<btScalar>(scale * radius),
static_cast<btScalar>(scale * height)));
return shape;
}
std::unique_ptr<btStaticPlaneShape> cWorld::BuildPlaneShape(const tVector& normal, const tVector& origin) const
{
btVector3 bt_normal = btVector3(static_cast<btScalar>(normal[0]),
static_cast<btScalar>(normal[1]),
static_cast<btScalar>(normal[2]));
btVector3 bt_origin = btVector3(static_cast<btScalar>(origin[0]),
static_cast<btScalar>(origin[1]),
static_cast<btScalar>(origin[2]));
bt_normal.normalize();
double scale = GetScale();
btScalar w = static_cast<btScalar>(scale * bt_normal.dot(bt_origin));
std::unique_ptr<btStaticPlaneShape> shape = std::unique_ptr<btStaticPlaneShape>(new btStaticPlaneShape(bt_normal, w));
return shape;
}
tVector cWorld::GetBoxSize(const cSimBox* box) const
{
const btBoxShape* box_shape = reinterpret_cast<btBoxShape*>(box->GetCollisionShape().get());
btVector3 half_len = box_shape->getHalfExtentsWithMargin();
double scale = GetScale();
return tVector(half_len[0] * 2, half_len[1] * 2, half_len[2] * 2, 0) / scale;
}
double cWorld::GetCapsuleHeight(const cSimCapsule* cap) const
{
const btCapsuleShape* shape = reinterpret_cast<btCapsuleShape*>(cap->GetCollisionShape().get());
double scale = GetScale();
double h = shape->getHalfHeight() * 2;
h /= scale;
return h;
}
double cWorld::GetCapsuleRadius(const cSimCapsule* cap) const
{
const btCapsuleShape* shape = reinterpret_cast<btCapsuleShape*>(cap->GetCollisionShape().get());
double scale = GetScale();
double r = shape->getRadius();
r /= scale;
return r;
}
tVector cWorld::GetPlaneCoeffs(const cSimPlane* plane) const
{
const btStaticPlaneShape* shape = reinterpret_cast<btStaticPlaneShape*>(plane->GetCollisionShape().get());
double scale = GetScale();
btVector3 n = shape->getPlaneNormal();
btScalar c = shape->getPlaneConstant();
return tVector(n[0], n[1], n[2], c / scale);
}
void cWorld::CalcRotationHinge(const cJoint* joint, tVector& out_axis, double& out_theta) const
{
btHingeConstraint* hinge = reinterpret_cast<btHingeConstraint*>(joint->GetConstraintHandle().mCons);
double ref_theta = joint->GetRefTheta();
out_theta = hinge->getHingeAngle();
out_theta = -out_theta; // theta flipped
// std::cout << "ref_theta " << ref_theta << std::endl;
out_theta -= ref_theta;
out_axis = joint->GetAxisRel();
}
double cWorld::CalcDisplacementPrismatic(const cJoint* joint) const
{
btSliderConstraint* prismatic = reinterpret_cast<btSliderConstraint*>(joint->GetConstraintHandle().mCons);
double scale = GetScale();
double delta = prismatic->getLinearPos();
delta = -delta;
delta /= scale;
return delta;
}
tVector cWorld::GetManifoldPtA(const btManifoldPoint& manifold_pt) const
{
double scale = GetScale();
const btVector3& contact_pt = manifold_pt.getPositionWorldOnA();
return tVector(contact_pt[0], contact_pt[1], contact_pt[2], 0) / scale;
}
tVector cWorld::GetManifoldPtB(const btManifoldPoint& manifold_pt) const
{
double scale = GetScale();
const btVector3& contact_pt = manifold_pt.getPositionWorldOnB();
return tVector(contact_pt[0], contact_pt[1], contact_pt[2], 0) / scale;
}
std::unique_ptr<btDiscreteDynamicsWorld>& cWorld::GetInternalWorld()
{
return mSimWorld;
}
void cWorld::ClearConstraints()
{
for (int i = GetNumConstriants() - 1; i >= 0; i--)
{
btTypedConstraint* constraint = mSimWorld->getConstraint(i);
mSimWorld->removeConstraint(constraint);
delete constraint;
}
}
int cWorld::GetNumConstriants() const
{
return static_cast<int>(mSimWorld->getNumConstraints());
}
cWorld::tConstraintHandle cWorld::AddHingeConstraint(cSimObj* obj0, cSimObj* obj1, const tJointParams& params)
{
btScalar scale = static_cast<btScalar>(GetScale());
btVector3 anchor0 = scale * btVector3(static_cast<btScalar>(params.mAnchor0[0]),
static_cast<btScalar>(params.mAnchor0[1]),
static_cast<btScalar>(params.mAnchor0[2]));
btVector3 anchor1 = scale * btVector3(static_cast<btScalar>(params.mAnchor1[0]),
static_cast<btScalar>(params.mAnchor1[1]),
static_cast<btScalar>(params.mAnchor1[2]));
btVector3 axis = btVector3(static_cast<btScalar>(params.mAxis[0]),
static_cast<btScalar>(params.mAxis[1]),
static_cast<btScalar>(params.mAxis[2]));
btHingeConstraint* cons = nullptr;
if (obj1 != nullptr)
{
cons = new btHingeConstraint(*(obj0->GetRigidBody()), *(obj1->GetRigidBody()),
anchor0, anchor1, axis, axis);
}
else
{
cons = new btHingeConstraint(*(obj0->GetRigidBody()), anchor0, axis);
}
// bullet limits are flipped
cons->setLimit(-static_cast<btScalar>(params.mLimHigh), -static_cast<btScalar>(params.mLimLow));
mSimWorld->addConstraint(cons, !params.mEnableAdjacentCollision);
tConstraintHandle handle;
handle.mCons = cons;
return handle;
}
cWorld::tConstraintHandle cWorld::AddPrismaticConstraint(cSimObj* obj0, cSimObj* obj1, const tJointParams& params)
{
btTransform anchor0_t;
anchor0_t.setIdentity();
btScalar scale = static_cast<btScalar>(GetScale());
btVector3 anchor0 = scale * btVector3(static_cast<btScalar>(params.mAnchor0[0]),
static_cast<btScalar>(params.mAnchor0[1]),
static_cast<btScalar>(params.mAnchor0[2]));
anchor0_t.setOrigin(anchor0);
anchor0_t.getBasis().setEulerZYX(0.f, 0.f, static_cast<btScalar>(M_PI_2));
btTransform anchor1_t;
anchor1_t.setIdentity();
btVector3 anchor1 = scale * btVector3(static_cast<btScalar>(params.mAnchor1[0]),
static_cast<btScalar>(params.mAnchor1[1]),
static_cast<btScalar>(params.mAnchor1[2]));
anchor1_t.setOrigin(anchor1);
anchor1_t.getBasis().setEulerZYX(0.f, 0.f, static_cast<btScalar>(M_PI_2));
btVector3 axis = btVector3(static_cast<btScalar>(params.mAxis[0]),
static_cast<btScalar>(params.mAxis[1]),
static_cast<btScalar>(params.mAxis[2]));
btSliderConstraint* cons = nullptr;
if (obj1 != nullptr)
{
cons = new btSliderConstraint(*(obj0->GetRigidBody()), *(obj1->GetRigidBody()),
anchor0_t, anchor1_t, true);
}
cons->setLowerLinLimit(static_cast<btScalar>(scale * params.mLimLow));
cons->setUpperLinLimit(static_cast<btScalar>(scale * params.mLimHigh));
cons->setLowerAngLimit(0.0f);
cons->setUpperAngLimit(0.0f);
mSimWorld->addConstraint(cons, !params.mEnableAdjacentCollision);
tConstraintHandle handle;
handle.mCons = cons;
return handle;
}