bullet3/examples/MultiBody/MultiBodySoftContact.cpp

149 lines
4.3 KiB
C++
Raw Normal View History

#include "MultiBodySoftContact.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
//static btScalar radius(0.2);
struct MultiBodySoftContact : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
bool m_once;
public:
MultiBodySoftContact(struct GUIHelperInterface* helper);
virtual ~MultiBodySoftContact();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = -21;
float yaw = 270;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper),
m_once(true)
{
}
MultiBodySoftContact::~MultiBodySoftContact()
{
}
void MultiBodySoftContact::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(50, 50, 50);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start;
start.setIdentity();
btVector3 groundOrigin(0, 0, -50.5);
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0, start, box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300, 10);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
{
btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
m_guiHelper->createCollisionShapeGraphicsObject(childShape);
btScalar mass = 1;
btVector3 baseInertiaDiag;
bool isFixed = (mass == 0);
childShape->calculateLocalInertia(mass, baseInertiaDiag);
btMultiBody* pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0, 0, 3));
pMultiBody->setBaseWorldTransform(startTrans);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(childShape);
pMultiBody->setBaseCollider(col);
bool isDynamic = (mass > 0 && !isFixed);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
pMultiBody->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(pMultiBody);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
pMultiBody->forwardKinematics(scratch_q, scratch_m);
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local, local_origin);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void MultiBodySoftContact::stepSimulation(float deltaTime)
{
if (0) //m_once)
{
m_once = false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n", torque, torque, torque); //[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(deltaTime);
}
class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
{
return new MultiBodySoftContact(options.m_guiHelper);
}