1
2 #include "MultiBodySoftContact.h"
3
4 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
5 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
6
7 #include "../CommonInterfaces/CommonMultiBodyBase.h"
8 #include "../Utils/b3ResourcePath.h"
9
10 //static btScalar radius(0.2);
11
12 struct MultiBodySoftContact : public CommonMultiBodyBase
13 {
14 btMultiBody* m_multiBody;
15 btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
16
17 bool m_once;
18
19 public:
20 MultiBodySoftContact(struct GUIHelperInterface* helper);
21 virtual ~MultiBodySoftContact();
22
23 virtual void initPhysics();
24
25 virtual void stepSimulation(float deltaTime);
26
resetCameraMultiBodySoftContact27 virtual void resetCamera()
28 {
29 float dist = 5;
30 float pitch = -21;
31 float yaw = 270;
32 float targetPos[3] = {0, 0, 0};
33 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
34 }
35 };
36
MultiBodySoftContact(struct GUIHelperInterface * helper)37 MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
38 : CommonMultiBodyBase(helper),
39 m_once(true)
40 {
41 }
42
~MultiBodySoftContact()43 MultiBodySoftContact::~MultiBodySoftContact()
44 {
45 }
46
initPhysics()47 void MultiBodySoftContact::initPhysics()
48 {
49 int upAxis = 2;
50
51 m_guiHelper->setUpAxis(upAxis);
52
53 btVector4 colors[4] =
54 {
55 btVector4(1, 0, 0, 1),
56 btVector4(0, 1, 0, 1),
57 btVector4(0, 1, 1, 1),
58 btVector4(1, 1, 0, 1),
59 };
60 int curColor = 0;
61
62 this->createEmptyDynamicsWorld();
63 m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
64
65 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
66 m_dynamicsWorld->getDebugDrawer()->setDebugMode(
67 //btIDebugDraw::DBG_DrawConstraints
68 +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
69
70 //create a static ground object
71 if (1)
72 {
73 btVector3 groundHalfExtents(50, 50, 50);
74 btBoxShape* box = new btBoxShape(groundHalfExtents);
75 box->initializePolyhedralFeatures();
76
77 m_guiHelper->createCollisionShapeGraphicsObject(box);
78 btTransform start;
79 start.setIdentity();
80 btVector3 groundOrigin(0, 0, -50.5);
81 start.setOrigin(groundOrigin);
82 // start.setRotation(groundOrn);
83 btRigidBody* body = createRigidBody(0, start, box);
84
85 //setContactStiffnessAndDamping will enable compliant rigid body contact
86 body->setContactStiffnessAndDamping(300, 10);
87 btVector4 color = colors[curColor];
88 curColor++;
89 curColor &= 3;
90 m_guiHelper->createRigidBodyGraphicsObject(body, color);
91 }
92
93 {
94 btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
95 m_guiHelper->createCollisionShapeGraphicsObject(childShape);
96
97 btScalar mass = 1;
98 btVector3 baseInertiaDiag;
99 bool isFixed = (mass == 0);
100 childShape->calculateLocalInertia(mass, baseInertiaDiag);
101 btMultiBody* pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
102 btTransform startTrans;
103 startTrans.setIdentity();
104 startTrans.setOrigin(btVector3(0, 0, 3));
105
106 pMultiBody->setBaseWorldTransform(startTrans);
107
108 btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
109 col->setCollisionShape(childShape);
110 pMultiBody->setBaseCollider(col);
111 bool isDynamic = (mass > 0 && !isFixed);
112 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
113 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
114
115 m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
116
117 pMultiBody->finalizeMultiDof();
118
119 m_dynamicsWorld->addMultiBody(pMultiBody);
120
121 btAlignedObjectArray<btQuaternion> scratch_q;
122 btAlignedObjectArray<btVector3> scratch_m;
123 pMultiBody->forwardKinematics(scratch_q, scratch_m);
124 btAlignedObjectArray<btQuaternion> world_to_local;
125 btAlignedObjectArray<btVector3> local_origin;
126 pMultiBody->updateCollisionObjectWorldTransforms(world_to_local, local_origin);
127 }
128 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
129 }
130
stepSimulation(float deltaTime)131 void MultiBodySoftContact::stepSimulation(float deltaTime)
132 {
133 if (/* DISABLES CODE */ (0)) //m_once)
134 {
135 m_once = false;
136 m_multiBody->addJointTorque(0, 10.0);
137
138 btScalar torque = m_multiBody->getJointTorque(0);
139 b3Printf("t = %f,%f,%f\n", torque, torque, torque); //[0],torque[1],torque[2]);
140 }
141
142 m_dynamicsWorld->stepSimulation(deltaTime);
143 }
144
MultiBodySoftContactCreateFunc(struct CommonExampleOptions & options)145 class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
146 {
147 return new MultiBodySoftContact(options.m_guiHelper);
148 }
149