Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -1,6 +1,5 @@
|
||||
#include "TestHingeTorque.h"
|
||||
|
||||
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
|
||||
@@ -10,99 +9,88 @@ static btScalar radius(0.2);
|
||||
|
||||
struct TestHingeTorque : public CommonRigidBodyBase
|
||||
{
|
||||
bool m_once;
|
||||
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
|
||||
bool m_once;
|
||||
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
|
||||
|
||||
TestHingeTorque(struct GUIHelperInterface* helper);
|
||||
virtual ~ TestHingeTorque();
|
||||
virtual ~TestHingeTorque();
|
||||
virtual void initPhysics();
|
||||
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
|
||||
|
||||
virtual void resetCamera()
|
||||
{
|
||||
|
||||
float dist = 5;
|
||||
float pitch = -21;
|
||||
float yaw = 270;
|
||||
float targetPos[3]={-1.34,3.4,-0.44};
|
||||
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
|
||||
float dist = 5;
|
||||
float pitch = -21;
|
||||
float yaw = 270;
|
||||
float targetPos[3] = {-1.34, 3.4, -0.44};
|
||||
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
TestHingeTorque::TestHingeTorque(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper),
|
||||
m_once(true)
|
||||
: CommonRigidBodyBase(helper),
|
||||
m_once(true)
|
||||
{
|
||||
}
|
||||
TestHingeTorque::~ TestHingeTorque()
|
||||
TestHingeTorque::~TestHingeTorque()
|
||||
{
|
||||
for (int i=0;i<m_jointFeedback.size();i++)
|
||||
for (int i = 0; i < m_jointFeedback.size(); i++)
|
||||
{
|
||||
delete m_jointFeedback[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void TestHingeTorque::stepSimulation(float deltaTime)
|
||||
{
|
||||
if (0)//m_once)
|
||||
{
|
||||
m_once=false;
|
||||
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
|
||||
|
||||
btRigidBody& bodyA = hinge->getRigidBodyA();
|
||||
btTransform trA = bodyA.getWorldTransform();
|
||||
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
|
||||
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
|
||||
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
|
||||
|
||||
}
|
||||
|
||||
m_dynamicsWorld->stepSimulation(1./240,0);
|
||||
|
||||
if (0) //m_once)
|
||||
{
|
||||
m_once = false;
|
||||
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
|
||||
|
||||
btRigidBody& bodyA = hinge->getRigidBodyA();
|
||||
btTransform trA = bodyA.getWorldTransform();
|
||||
btVector3 hingeAxisInWorld = trA.getBasis() * hinge->getFrameOffsetA().getBasis().getColumn(2);
|
||||
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld * 10);
|
||||
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld * 10);
|
||||
}
|
||||
|
||||
m_dynamicsWorld->stepSimulation(1. / 240, 0);
|
||||
|
||||
static int count = 0;
|
||||
if ((count& 0x0f)==0)
|
||||
if ((count & 0x0f) == 0)
|
||||
{
|
||||
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
|
||||
|
||||
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
|
||||
|
||||
b3Printf("base angvel = %f,%f,%f", base->getAngularVelocity()[0],
|
||||
base->getAngularVelocity()[1],
|
||||
|
||||
|
||||
base->getAngularVelocity()[2]);
|
||||
|
||||
|
||||
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
|
||||
|
||||
|
||||
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
|
||||
|
||||
b3Printf("child angvel = %f,%f,%f", child->getAngularVelocity()[0],
|
||||
child->getAngularVelocity()[1],
|
||||
|
||||
child->getAngularVelocity()[2]);
|
||||
|
||||
for (int i=0;i<m_jointFeedback.size();i++)
|
||||
|
||||
for (int i = 0; i < m_jointFeedback.size(); i++)
|
||||
{
|
||||
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
|
||||
|
||||
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.z(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.z(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
|
||||
}
|
||||
}
|
||||
count++;
|
||||
|
||||
//CommonRigidBodyBase::stepSimulation(deltaTime);
|
||||
//CommonRigidBodyBase::stepSimulation(deltaTime);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void TestHingeTorque::initPhysics()
|
||||
{
|
||||
int upAxis = 1;
|
||||
@@ -110,92 +98,89 @@ void TestHingeTorque::initPhysics()
|
||||
|
||||
createEmptyDynamicsWorld();
|
||||
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
|
||||
|
||||
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
|
||||
|
||||
|
||||
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
|
||||
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
int mode = btIDebugDraw::DBG_DrawWireframe
|
||||
+btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawConstraintLimits;
|
||||
int mode = btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits;
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
|
||||
|
||||
{ // create a door using hinge constraint attached to the world
|
||||
|
||||
{ // create a door using hinge constraint attached to the world
|
||||
|
||||
int numLinks = 2;
|
||||
// bool selfCollide = false;
|
||||
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
|
||||
int numLinks = 2;
|
||||
// bool selfCollide = false;
|
||||
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
|
||||
|
||||
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
|
||||
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
btTransform baseWorldTrans;
|
||||
baseWorldTrans.setIdentity();
|
||||
baseWorldTrans.setOrigin(basePosition);
|
||||
|
||||
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
|
||||
//init the base
|
||||
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
|
||||
float baseMass = 0.f;
|
||||
float linkMass = 1.f;
|
||||
|
||||
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
|
||||
m_dynamicsWorld->removeRigidBody(base);
|
||||
base->setDamping(0,0);
|
||||
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
|
||||
btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
|
||||
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
|
||||
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
btTransform baseWorldTrans;
|
||||
baseWorldTrans.setIdentity();
|
||||
baseWorldTrans.setOrigin(basePosition);
|
||||
|
||||
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
|
||||
//init the base
|
||||
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
|
||||
float baseMass = 0.f;
|
||||
float linkMass = 1.f;
|
||||
|
||||
btRigidBody* base = createRigidBody(baseMass, baseWorldTrans, baseBox);
|
||||
m_dynamicsWorld->removeRigidBody(base);
|
||||
base->setDamping(0, 0);
|
||||
m_dynamicsWorld->addRigidBody(base, collisionFilterGroup, collisionFilterMask);
|
||||
btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
|
||||
btSphereShape* linkSphere = new btSphereShape(radius);
|
||||
|
||||
btRigidBody* prevBody = base;
|
||||
|
||||
for (int i=0;i<numLinks;i++)
|
||||
{
|
||||
btTransform linkTrans;
|
||||
linkTrans = baseWorldTrans;
|
||||
|
||||
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
|
||||
|
||||
|
||||
btRigidBody* prevBody = base;
|
||||
|
||||
for (int i = 0; i < numLinks; i++)
|
||||
{
|
||||
btTransform linkTrans;
|
||||
linkTrans = baseWorldTrans;
|
||||
|
||||
linkTrans.setOrigin(basePosition - btVector3(0, linkHalfExtents[1] * 2.f * (i + 1), 0));
|
||||
|
||||
btCollisionShape* colOb = 0;
|
||||
|
||||
if (i==0)
|
||||
|
||||
if (i == 0)
|
||||
{
|
||||
colOb = linkBox1;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
colOb = linkSphere;
|
||||
}
|
||||
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb);
|
||||
m_dynamicsWorld->removeRigidBody(linkBody);
|
||||
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
|
||||
linkBody->setDamping(0,0);
|
||||
btRigidBody* linkBody = createRigidBody(linkMass, linkTrans, colOb);
|
||||
m_dynamicsWorld->removeRigidBody(linkBody);
|
||||
m_dynamicsWorld->addRigidBody(linkBody, collisionFilterGroup, collisionFilterMask);
|
||||
linkBody->setDamping(0, 0);
|
||||
btTypedConstraint* con = 0;
|
||||
|
||||
if (i==0)
|
||||
|
||||
if (i == 0)
|
||||
{
|
||||
//create a hinge constraint
|
||||
btVector3 pivotInA(0,-linkHalfExtents[1],0);
|
||||
btVector3 pivotInB(0,linkHalfExtents[1],0);
|
||||
btVector3 axisInA(1,0,0);
|
||||
btVector3 axisInB(1,0,0);
|
||||
btVector3 pivotInA(0, -linkHalfExtents[1], 0);
|
||||
btVector3 pivotInB(0, linkHalfExtents[1], 0);
|
||||
btVector3 axisInA(1, 0, 0);
|
||||
btVector3 axisInB(1, 0, 0);
|
||||
bool useReferenceA = true;
|
||||
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
|
||||
pivotInA,pivotInB,
|
||||
axisInA,axisInB,useReferenceA);
|
||||
btHingeConstraint* hinge = new btHingeConstraint(*prevBody, *linkBody,
|
||||
pivotInA, pivotInB,
|
||||
axisInA, axisInB, useReferenceA);
|
||||
con = hinge;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset
|
||||
btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset
|
||||
btTransform pivotInA(btQuaternion::getIdentity(), btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset
|
||||
btTransform pivotInB(btQuaternion::getIdentity(), btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset
|
||||
btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody,
|
||||
pivotInA,pivotInB);
|
||||
fixed->setLinearLowerLimit(btVector3(0,0,0));
|
||||
fixed->setLinearUpperLimit(btVector3(0,0,0));
|
||||
fixed->setAngularLowerLimit(btVector3(0,0,0));
|
||||
fixed->setAngularUpperLimit(btVector3(0,0,0));
|
||||
|
||||
con = fixed;
|
||||
pivotInA, pivotInB);
|
||||
fixed->setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
fixed->setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
fixed->setAngularLowerLimit(btVector3(0, 0, 0));
|
||||
fixed->setAngularUpperLimit(btVector3(0, 0, 0));
|
||||
|
||||
con = fixed;
|
||||
}
|
||||
btAssert(con);
|
||||
if (con)
|
||||
@@ -204,38 +189,36 @@ void TestHingeTorque::initPhysics()
|
||||
m_jointFeedback.push_back(fb);
|
||||
con->setJointFeedback(fb);
|
||||
|
||||
m_dynamicsWorld->addConstraint(con,true);
|
||||
m_dynamicsWorld->addConstraint(con, true);
|
||||
}
|
||||
prevBody = linkBody;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (1)
|
||||
{
|
||||
btVector3 groundHalfExtents(1,1,0.2);
|
||||
groundHalfExtents[upAxis]=1.f;
|
||||
btVector3 groundHalfExtents(1, 1, 0.2);
|
||||
groundHalfExtents[upAxis] = 1.f;
|
||||
btBoxShape* box = new btBoxShape(groundHalfExtents);
|
||||
box->initializePolyhedralFeatures();
|
||||
|
||||
btTransform start; start.setIdentity();
|
||||
|
||||
btTransform start;
|
||||
start.setIdentity();
|
||||
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
|
||||
// btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
|
||||
|
||||
groundOrigin[upAxis] -=.5;
|
||||
groundOrigin[2]-=0.6;
|
||||
// btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
btQuaternion groundOrn(btVector3(0, 1, 0), 0.25 * SIMD_PI);
|
||||
|
||||
groundOrigin[upAxis] -= .5;
|
||||
groundOrigin[2] -= 0.6;
|
||||
start.setOrigin(groundOrigin);
|
||||
// start.setRotation(groundOrn);
|
||||
btRigidBody* body = createRigidBody(0,start,box);
|
||||
// start.setRotation(groundOrn);
|
||||
btRigidBody* body = createRigidBody(0, start, box);
|
||||
body->setFriction(0);
|
||||
|
||||
}
|
||||
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||
}
|
||||
|
||||
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
|
||||
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
|
||||
{
|
||||
return new TestHingeTorque(options.m_guiHelper);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user