From a159fbac698520b7fcca585d6129eaa79bcdd69b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 22 Jan 2015 17:56:24 -0800 Subject: [PATCH] Improved URDF support for btMultiBody and separate graphics/collision/inertial frames and shapes Fix WinXP GetTickCount64 with a typedef Expose debug drawing mode/flags in UI (hot keys A,D,L,W for now, buttons later) GLInstancingRenderer: tweak near/far planes to allow closer approach of camera btDiscreteDynamicsWorld: enable debug drawing for btGeneric6DofSpring2Constraint btMultiBodyDynamicsWorld: enable basic debug drawing for btMultiBody btMultibody: allow center-of-mass shift for prismatic and fixed constraint --- Demos3/AllBullet2Demos/main.cpp | 26 +- .../FiniteElementMethod/FiniteElementDemo.cpp | 2 +- .../FiniteElementMethod/FiniteElementDemo.h | 2 +- Demos3/Geometry/DistributePoints.h | 2 +- Demos3/Geometry/SphereCreation.h | 2 +- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 735 +++++++++--------- Demos3/ImportURDFDemo/urdf_samples.h | 680 +++++++++------- .../BasicConcepts/CoordinateSystemDemo.h | 2 +- .../CollisionDetection/SupportFuncDemo.h | 2 +- .../BulletMultiBodyDemos.cpp | 3 +- .../BulletMultiBodyDemos.h | 2 +- Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp | 8 +- Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h | 2 +- .../Bullet2RigidBodyDemo.cpp | 4 +- .../Bullet3AppSupport/Bullet2RigidBodyDemo.h | 2 +- btgui/Bullet3AppSupport/BulletDemoInterface.h | 5 +- .../Bullet3AppSupport/CommonMultiBodySetup.h | 6 +- btgui/Bullet3AppSupport/CommonPhysicsSetup.h | 2 +- .../Bullet3AppSupport/CommonRigidBodySetup.h | 6 +- btgui/OpenGLWindow/GLInstancingRenderer.cpp | 4 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 52 ++ .../Dynamics/btDiscreteDynamicsWorld.h | 2 +- .../Featherstone/btMultiBody.cpp | 43 +- src/BulletDynamics/Featherstone/btMultiBody.h | 3 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 80 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 2 + .../Featherstone/btMultiBodyLink.h | 12 +- src/LinearMath/btQuickprof.cpp | 5 + 28 files changed, 1022 insertions(+), 674 deletions(-) diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index a6f23f69b..41c7e0b19 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -234,9 +234,10 @@ extern bool useShadowMap; static bool visualWireframe=false; static bool renderVisualGeometry=true; static bool renderGrid = true; - -static bool pauseSimulation=false;//true; +int gDebugDrawFlags = 0; +static bool pauseSimulation=true; int midiBaseIndex = 176; +extern bool gDisableDeactivation; //#include //unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM); @@ -268,9 +269,28 @@ void MyKeyboardCallback(int key, int state) //if (handled) // return; + if (key=='a' && state) + { + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawAabb; + } + if (key=='c' && state) + { + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints; + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawContactPoints; + } + if (key == 'd' && state) + { + gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation; + gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0); + } + if (key=='l' && state) + { + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraintLimits; + } if (key=='w' && state) { visualWireframe=!visualWireframe; + gDebugDrawFlags ^= btIDebugDraw::DBG_DrawWireframe; } if (key=='v' && state) { @@ -792,7 +812,7 @@ int main(int argc, char* argv[]) } { glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - sCurrentDemo->physicsDebugDraw(); + sCurrentDemo->physicsDebugDraw(gDebugDrawFlags); } } diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp index 04b5b7602..8612e1f08 100644 --- a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp @@ -317,7 +317,7 @@ void FiniteElementDemo::renderScene() m_app->m_renderer->renderScene(); } -void FiniteElementDemo::physicsDebugDraw() +void FiniteElementDemo::physicsDebugDraw(int debugDrawFlags) { { btAlignedObjectArray m_linePoints; diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.h b/Demos3/FiniteElementMethod/FiniteElementDemo.h index 9a4a5cc49..59ab3fb19 100644 --- a/Demos3/FiniteElementMethod/FiniteElementDemo.h +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.h @@ -50,7 +50,7 @@ public: virtual void renderScene(); - virtual void physicsDebugDraw(); + virtual void physicsDebugDraw(int debugDrawFlags); virtual bool mouseMoveCallback(float x,float y); virtual bool mouseButtonCallback(int button, int state, float x, float y); virtual bool keyboardCallback(int key, int state); diff --git a/Demos3/Geometry/DistributePoints.h b/Demos3/Geometry/DistributePoints.h index facf8a3bc..23213468c 100644 --- a/Demos3/Geometry/DistributePoints.h +++ b/Demos3/Geometry/DistributePoints.h @@ -181,7 +181,7 @@ public: } - virtual void physicsDebugDraw() + virtual void physicsDebugDraw(int debugDrawFlags) { int lineWidth = 1; diff --git a/Demos3/Geometry/SphereCreation.h b/Demos3/Geometry/SphereCreation.h index 8febf629f..bb832cecb 100644 --- a/Demos3/Geometry/SphereCreation.h +++ b/Demos3/Geometry/SphereCreation.h @@ -168,7 +168,7 @@ public: m_app->m_renderer->renderScene(); } - virtual void physicsDebugDraw() + virtual void physicsDebugDraw(int debugDrawFlags) { int lineWidth = 1; int pointSize = 2; diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index 82113e2e8..964729a7f 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -12,9 +12,10 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr static bool enableConstraints = true;//false; + ImportUrdfSetup::ImportUrdfSetup() { - sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");// + sprintf(m_fileName,"r2d2.urdf"); } ImportUrdfSetup::~ImportUrdfSetup() @@ -86,12 +87,26 @@ struct URDF_LinkInformation { const Link* m_thisLink; int m_linkIndex; - int m_parentIndex; + //int m_parentIndex; btTransform m_localInertialFrame; - btTransform m_localVisualFrame; + //btTransform m_localVisualFrame; + btTransform m_bodyWorldTransform; + btVector3 m_localInertiaDiagonal; + btScalar m_mass; + btCollisionShape* m_collisionShape; btRigidBody* m_bulletRigidBody; + + URDF_LinkInformation() + :m_thisLink(0), + m_linkIndex(-2), + //m_parentIndex(-2), + m_collisionShape(0), + m_bulletRigidBody(0) + { + + } virtual ~URDF_LinkInformation() { printf("~\n"); @@ -107,17 +122,30 @@ struct URDF_JointInformation struct URDF2BulletMappings { btHashMap m_link2rigidbody; - btHashMap m_joint2Constraint; + //btHashMap m_joint2Constraint; - btAlignedObjectArray m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia; + //btAlignedObjectArray m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia; btAlignedObjectArray m_linkMasses; - btAlignedObjectArray m_linkLocalDiagonalInertiaTensors; - btAlignedObjectArray m_jointTransforms;//for root, it is identity - btAlignedObjectArray m_parentIndices;//for root, it is identity - btAlignedObjectArray m_jointAxisArray; - btAlignedObjectArray m_jointOffsetInParent; - btAlignedObjectArray m_jointOffsetInChild; - btAlignedObjectArray m_jointTypeArray; + //btAlignedObjectArray m_linkLocalDiagonalInertiaTensors; + + //btAlignedObjectArray m_parentIndices;//for root, it is identity + //btAlignedObjectArray m_jointAxisArray; + //btAlignedObjectArray m_jointOffsetInParent; + //btAlignedObjectArray m_jointOffsetInChild; + //btAlignedObjectArray m_jointTypeArray; + + + + bool m_createMultiBody; + int m_totalNumJoints; + btMultiBody* m_bulletMultiBody; + + URDF2BulletMappings() + :m_createMultiBody(false), + m_totalNumJoints(0), + m_bulletMultiBody(0) + { + } }; enum MyFileType @@ -126,7 +154,8 @@ enum MyFileType FILE_COLLADA=2 }; -btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const char* pathPrefix) +template +btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix) { btCollisionShape* shape = 0; @@ -151,6 +180,8 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->initializePolyhedralFeatures(); + //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); cylZShape->setMargin(0.001); @@ -164,6 +195,7 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c urdf::Box* box = (urdf::Box*)visual->geometry.get(); btVector3 extents(box->dim.x,box->dim.y,box->dim.z); btBoxShape* boxShape = new btBoxShape(extents*0.5f); + //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); shape = boxShape; shape ->setMargin(0.001); break; @@ -348,32 +380,46 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c } return shape; } -void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) +void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) { - btCollisionShape* shape = 0; + //btCollisionShape* shape = 0; btTransform linkTransformInWorldSpace; linkTransformInWorldSpace.setIdentity(); - btScalar mass = 1; + btScalar mass = 0; btTransform inertialFrame; inertialFrame.setIdentity(); const Link* parentLink = (*link).getParent(); URDF_LinkInformation* pp = 0; + int linkIndex = mappings.m_linkMasses.size(); + btVector3 localInertiaDiagonal(0,0,0); + + int parentIndex = -1; + + + if (parentLink) + { + parentIndex = parentLink->m_link_index; + btAssert(parentIndex>=0); + } { URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink); if (ppRigidBody) { - pp = (*ppRigidBody); - btRigidBody* parentRigidBody = pp->m_bulletRigidBody; - btTransform tr = parentRigidBody->getWorldTransform(); + pp = (*ppRigidBody); + btTransform tr = pp->m_bodyWorldTransform; printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z()); } } + + (*link).m_link_index = linkIndex; + if ((*link).inertial) { mass = (*link).inertial->mass; + localInertiaDiagonal.setValue((*link).inertial->ixx,(*link).inertial->iyy,(*link).inertial->izz); inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z)); inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w)); } @@ -394,54 +440,123 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy } else { linkTransformInWorldSpace = parentTransformInWorldSpace; + + + } { printf("converting visuals of link %s",link->name.c_str()); - for (int v=0;v<(int)link->collision_array.size();v++) + + + { - const Collision* visual = link->collision_array[v].get(); - shape = convertVisualToCollisionShape(visual,pathPrefix); - if (shape) - { - gfxBridge.createCollisionShapeGraphicsObject(shape); + btCompoundShape* tmpGfxShape = new btCompoundShape(); - btVector3 color = selectColor(); -/* if (visual->material.get()) - { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); - } - */ - btVector3 localInertia(0,0,0); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia); + for (int v=0;v<(int)link->visual_array.size();v++) + { + const Visual* vis = link->visual_array[v].get(); + btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix); + if (childShape) + { + btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z); + btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w); + btTransform childTrans; + childTrans.setOrigin(childPos); + childTrans.setRotation(childOrn); + if (!mappings.m_createMultiBody) + { + tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape); + } else + { + tmpGfxShape->addChildShape(childTrans,childShape); + } + } + } + + btCompoundShape* compoundShape = new btCompoundShape(); + for (int v=0;v<(int)link->collision_array.size();v++) + { + const Collision* col = link->collision_array[v].get(); + btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix); + if (childShape) + { + btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z); + btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w); + btTransform childTrans; + childTrans.setOrigin(childPos); + childTrans.setRotation(childOrn); + if (!mappings.m_createMultiBody) + { + compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape); + } else + { + compoundShape->addChildShape(childTrans,childShape); + } + + } + } + + + + + + if (compoundShape) + { + + + btVector3 color = selectColor(); + /* if (visual->material.get()) + { + color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); + } + */ + //btVector3 localInertiaDiagonal(0, 0, 0); + //if (mass) + //{ + // shape->calculateLocalInertia(mass, localInertiaDiagonal); + //} + + + //btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame; + btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame; + URDF_LinkInformation* linkInfo = new URDF_LinkInformation; + + if (!mappings.m_createMultiBody) + { + btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal); + rbci.m_startWorldTransform = inertialFrameInWorldSpace; + linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace + //rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass; + btRigidBody* body = new btRigidBody(rbci); + world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); + gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape); + //hack-> transfer user inder from visual to collision shape + compoundShape->setUserIndex(tmpGfxShape->getUserIndex()); + + gfxBridge.createRigidBodyGraphicsObject(body, color); + linkInfo->m_bulletRigidBody = body; + } else + { + if (mappings.m_bulletMultiBody==0) + { + bool multiDof = true; + bool canSleep = false; + bool isFixedBase = (mass==0);//todo: figure out when base is fixed + int totalNumJoints = mappings.m_totalNumJoints; + mappings.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); + } + + } - btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z); - btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w); - btTransform visual_frame; - visual_frame.setOrigin(visual_pos); - visual_frame.setRotation(visual_orn); - - btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame; - rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass; - - - btRigidBody* body = new btRigidBody(rbci); - - world1->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask); - // body->setFriction(0); - - gfxBridge.createRigidBodyGraphicsObject(body,color); - URDF_LinkInformation* linkInfo = new URDF_LinkInformation; - linkInfo->m_bulletRigidBody = body; - linkInfo->m_localVisualFrame =visual_frame; + linkInfo->m_collisionShape = compoundShape; + linkInfo->m_localInertiaDiagonal = localInertiaDiagonal; + linkInfo->m_mass = mass; + //linkInfo->m_localVisualFrame =visual_frame; linkInfo->m_localInertialFrame =inertialFrame; linkInfo->m_thisLink = link.get(); const Link* p = link.get(); @@ -452,68 +567,173 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { btAssert(pp); - btRigidBody* parentBody =pp->m_bulletRigidBody; + const Joint* pj = (*link).parent_joint.get(); btTransform offsetInA,offsetInB; - + static bool once = true; + offsetInA.setIdentity(); - - offsetInA = pp->m_localVisualFrame.inverse()*parent2joint; + static bool toggle=false; + + //offsetInA = pp->m_localVisualFrame.inverse()*parent2joint; + offsetInA = pp->m_localInertialFrame.inverse()*parent2joint; + offsetInB.setIdentity(); - offsetInB = visual_frame.inverse(); - + //offsetInB = visual_frame.inverse(); + offsetInB = inertialFrame.inverse(); + + + bool disableParentCollision = true; + btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z); switch (pj->type) { case Joint::FIXED: { - printf("Fixed joint\n"); - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); -// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); + if (mappings.m_createMultiBody) + { + //todo: adjust the center of mass transform and pivot axis properly - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); + printf("Fixed joint (btMultiBody)\n"); + //btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin()); + btQuaternion rot = parent2joint.inverse().getRotation(); + //toggle=!toggle; + mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, + rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision); + btMatrix3x3 rm(rot); + btScalar y,p,r; + rm.getEulerZYX(y,p,r); + //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); + //linkInfo->m_localVisualFrame.setIdentity(); + printf("y=%f,p=%f,r=%f\n", y,p,r); + - if (enableConstraints) - world1->addConstraint(dof6,true); -// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); - // world->addConstraint(fixed,true); + } else + { + printf("Fixed joint\n"); + + btMatrix3x3 rm(offsetInA.getBasis()); + btScalar y,p,r; + rm.getEulerZYX(y,p,r); + //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); + //linkInfo->m_localVisualFrame.setIdentity(); + printf("y=%f,p=%f,r=%f\n", y,p,r); + + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB); + // btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + + if (enableConstraints) + world1->addConstraint(dof6,true); + + // btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); + // world->addConstraint(fixed,true); + } break; } case Joint::CONTINUOUS: case Joint::REVOLUTE: { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); -// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); + if (mappings.m_createMultiBody) + { + //todo: adjust the center of mass transform and pivot axis properly + mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, + parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(), + btVector3(0,0,0),//offsetInB.getOrigin(), + disableParentCollision); - dof6->setAngularLowerLimit(btVector3(0,0,1000)); - dof6->setAngularUpperLimit(btVector3(0,0,-1000)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - - printf("Revolute/Continuous joint\n"); + /* + + mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, + parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(), + -offsetInB.getOrigin(), + disableParentCollision); + linkInfo->m_localVisualFrame.setIdentity(); + */ + } else + { + //only handle principle axis at the moment, + //@todo(erwincoumans) orient the constraint for non-principal axis + btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z); + int principleAxis = axis.closestAxis(); + switch (principleAxis) + { + case 0: + { + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_ZYX); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(-1,0,0)); + dof6->setAngularLowerLimit(btVector3(1,0,0)); + + if (enableConstraints) + world1->addConstraint(dof6,true); + break; + } + case 1: + { + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XZY); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(0,-1,0)); + dof6->setAngularLowerLimit(btVector3(0,1,0)); + + if (enableConstraints) + world1->addConstraint(dof6,true); + break; + } + case 2: + default: + { + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XYZ); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(0,0,-1)); + dof6->setAngularLowerLimit(btVector3(0,0,0)); + + if (enableConstraints) + world1->addConstraint(dof6,true); + } + }; + printf("Revolute/Continuous joint\n"); + } break; } case Joint::PRISMATIC: { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); + if (mappings.m_createMultiBody) + { + mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, + parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision); - dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0)); - dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0)); + //mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, + // parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(), + // offsetInB.getOrigin(), + // disableParentCollision); - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); + } else + { + + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB); + dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0)); + dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0)); - if (enableConstraints) - world1->addConstraint(dof6,true); + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); - printf("Prismatic\n"); + if (enableConstraints) + world1->addConstraint(dof6,true); + + printf("Prismatic\n"); + } break; } default: @@ -523,10 +743,63 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy } } + + if (mappings.m_createMultiBody) + { + if (compoundShape->getNumChildShapes()>0) + { + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mappings.m_bulletMultiBody, linkIndex-1); + + //btCompoundShape* comp = new btCompoundShape(); + //comp->addChildShape(linkInfo->m_localVisualFrame,shape); + + gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape); + compoundShape->setUserIndex(tmpGfxShape->getUserIndex()); + col->setCollisionShape(compoundShape); + + btTransform tr; + tr.setIdentity(); + tr = linkTransformInWorldSpace; + //if we don't set the initial pose of the btCollisionObject, the simulator will do this + //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider + + //tr.setOrigin(local_origin[0]); + //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + + bool isDynamic = true; + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); + + btVector3 color = selectColor();//(0.0,0.0,0.5); + + gfxBridge.createCollisionObjectGraphicsObject(col,color); + btScalar friction = 0.5f; + + col->setFriction(friction); + + if (parentIndex>=0) + { + mappings.m_bulletMultiBody->getLink(linkIndex-1).m_collider=col; + } else + { + mappings.m_bulletMultiBody->setBaseCollider(col); + } + } + } + + //mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal); + //mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform); } + + } } + mappings.m_linkMasses.push_back(mass); + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) { if (*child) @@ -546,257 +819,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy } - -btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints) -{ - - btScalar mass = 0.f; - btTransform localInertialTransform; localInertialTransform.setIdentity(); - btVector3 localInertiaDiagonal(0,0,0); - - { - - if ((*link).inertial) - { - mass = (*link).inertial->mass; - btMatrix3x3 inertiaMat; - inertiaMat.setIdentity(); - inertiaMat.setValue( - (*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz, - (*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz, - (*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz); - - btScalar threshold = 0.00001f; - int maxSteps=20; - btMatrix3x3 inertia2PrincipalAxis; - inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps); - localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]); - - btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z); - localInertialTransform.setOrigin(inertiaLocalCOM); - btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w); - btMatrix3x3 inertiaOrnMat(inertiaOrn); - - if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f - || localInertiaDiagonal[2] == 0.f)) - { - b3Warning("Error: inertia should not be zero if mass is positive\n"); - localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1)); - localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat); - } - else - { - localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis); - } - } - } - btTransform linkTransformInWorldSpace; - int parentIndex = -1; - - const Link* parentLink = (*link).getParent(); - if (parentLink) - { - parentIndex = parentLink->m_link_index; - btAssert(parentIndex>=0); - } - int linkIndex = mappings.m_linkMasses.size(); - - btTransform parent2joint; - - if ((*link).parent_joint) - { - const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation; - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - } else - { - linkTransformInWorldSpace = parentTransformInWorldSpace; - - btAssert(mb==0); - - bool multiDof = true; - bool canSleep = false; - bool isFixedBase = (mass==0);//todo: figure out when base is fixed - - mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); - - - } - - btAssert(mb); - - (*link).m_link_index = linkIndex; - - //compute this links center of mass transform, aligned with the principal axis of inertia - - - { - //btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame; - - mappings.m_linkMasses.push_back(mass); - mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal); - mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform); - - - - if ((*link).parent_joint) - { - btTransform offsetInA,offsetInB; - offsetInA.setIdentity(); - //offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint; - offsetInA = parent2joint; - offsetInB.setIdentity(); - //offsetInB = localInertialTransform.inverse(); - - const Joint* pj = (*link).parent_joint.get(); - //btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z); - btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z); - mappings.m_jointAxisArray.push_back(jointAxis); - mappings.m_jointOffsetInParent.push_back(offsetInA); - mappings.m_jointOffsetInChild.push_back(offsetInB); - mappings.m_jointTypeArray.push_back(pj->type); - - switch (pj->type) - { - case Joint::FIXED: - { - printf("Fixed joint\n"); - mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin()); - - break; - } - case Joint::CONTINUOUS: - case Joint::REVOLUTE: - { - printf("Revolute joint\n"); - mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true); - mb->finalizeMultiDof(); - //mb->setJointVel(linkIndex-1,1); - - break; - } - case Joint::PRISMATIC: - { - mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true); - printf("Prismatic joint\n"); - break; - } - default: - { - printf("Unknown joint\n"); - btAssert(0); - } - }; - - - - - } else - { - mappings.m_jointAxisArray.push_back(btVector3(0,0,0)); - btTransform ident; - ident.setIdentity(); - mappings.m_jointOffsetInParent.push_back(ident); - mappings.m_jointOffsetInChild.push_back(ident); - mappings.m_jointTypeArray.push_back(-1); - - - } - } - - //btCompoundShape* compoundShape = new btCompoundShape(); - btCollisionShape* shape = 0; - - for (int v=0;v<(int)link->collision_array.size();v++) - { - const Collision* visual = link->collision_array[v].get(); - - shape = convertVisualToCollisionShape(visual,pathPrefix); - - if (shape)//childShape) - { - gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape); - - //btVector3 color = selectColor(); - /* - if (visual->material.get()) - { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); - } - */ - btVector3 localInertia(0,0,0); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - //btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia); - - - btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z); - btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w); - btTransform visual_frame; - visual_frame.setOrigin(visual_pos); - visual_frame.setRotation(visual_orn); - btTransform childTransform; - childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform - // compoundShape->addChildShape(childTransform,childShape); - } - } - - if (shape)//compoundShape->getNumChildShapes()>0) - { - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1); - col->setCollisionShape(shape); - - btTransform tr; - tr.setIdentity(); - tr = linkTransformInWorldSpace; - //if we don't set the initial pose of the btCollisionObject, the simulator will do this - //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider - - //tr.setOrigin(local_origin[0]); - //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - - bool isDynamic = true; - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); - - btVector3 color = selectColor();//(0.0,0.0,0.5); - gfxBridge.createCollisionObjectGraphicsObject(col,color); - btScalar friction = 0.5f; - - col->setFriction(friction); - - if (parentIndex>=0) - { - mb->getLink(linkIndex-1).m_collider=col; - } else - { - mb->setBaseCollider(col); - } - } - - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints); - - } - else - { - std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; - } - } - return mb; -} - - void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { @@ -874,27 +896,22 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) int numJoints = (*robot).m_numJoints; static bool useFeatherstone = false; - if (!useFeatherstone) { URDF2BulletMappings mappings; + mappings.m_createMultiBody = useFeatherstone; + mappings.m_totalNumJoints = numJoints; URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix); + if (useFeatherstone) + { + btMultiBody* mb = mappings.m_bulletMultiBody; + mb->setHasSelfCollision(false); + mb->finalizeMultiDof(); + m_dynamicsWorld->addMultiBody(mb); + } } //the btMultiBody support is work-in-progress :-) -#if 1 - else - { - URDF2BulletMappings mappings; - - btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints); - - mb->setHasSelfCollision(false); - mb->finalizeMultiDof(); - m_dynamicsWorld->addMultiBody(mb); - //m_dynamicsWorld->integrateTransforms(0.f); - } -#endif// useFeatherstone = !useFeatherstone; printf("numJoints/DOFS = %d\n", numJoints); @@ -923,7 +940,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) gfxBridge.createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-2.5; + groundOrigin[upAxis]=-1.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); @@ -932,7 +949,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) gfxBridge.createRigidBodyGraphicsObject(body,color); } - + m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); } void ImportUrdfSetup::stepSimulation(float deltaTime) diff --git a/Demos3/ImportURDFDemo/urdf_samples.h b/Demos3/ImportURDFDemo/urdf_samples.h index 7a5253273..24d4d2c0c 100644 --- a/Demos3/ImportURDFDemo/urdf_samples.h +++ b/Demos3/ImportURDFDemo/urdf_samples.h @@ -401,269 +401,423 @@ const char* urdf_char_r2d2 = MSTRINGIFY( const char* urdf_char = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ); diff --git a/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h b/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h index 69e3dc229..f6aedb83b 100644 --- a/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h +++ b/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h @@ -89,7 +89,7 @@ public: } } - virtual void physicsDebugDraw() + virtual void physicsDebugDraw(int debugDrawFlags) { btVector3 xUnit(1,0,0); diff --git a/Demos3/bullet2/CollisionDetection/SupportFuncDemo.h b/Demos3/bullet2/CollisionDetection/SupportFuncDemo.h index ad26b2d66..d6e5f0354 100644 --- a/Demos3/bullet2/CollisionDetection/SupportFuncDemo.h +++ b/Demos3/bullet2/CollisionDetection/SupportFuncDemo.h @@ -74,7 +74,7 @@ public: m_app->m_renderer->renderScene(); } - virtual void physicsDebugDraw() + virtual void physicsDebugDraw(int debugDrawFlags) { int width=3; btVector3 from(0,0,0); diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 3a20a8ddf..f652ba625 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -612,10 +612,11 @@ void FeatherstoneDemo1::renderScene() m_glApp->m_renderer->renderScene(); } -void FeatherstoneDemo1::physicsDebugDraw() +void FeatherstoneDemo1::physicsDebugDraw(int debugDrawFlags) { if (m_dynamicsWorld) { + m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); m_dynamicsWorld->debugDrawWorld(); } } diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h index 295a13e55..eb3a6aba7 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h @@ -89,7 +89,7 @@ public: virtual void initPhysics(); virtual void exitPhysics(); virtual void renderScene(); - virtual void physicsDebugDraw(); + virtual void physicsDebugDraw(int debugDrawFlags); virtual void stepSimulation(float deltaTime); }; diff --git a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp index 58fe896a7..5b3a756e6 100644 --- a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp +++ b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp @@ -413,10 +413,16 @@ void LuaPhysicsSetup::stepSimulation(float deltaTime) } -void LuaPhysicsSetup::debugDraw() +void LuaPhysicsSetup::debugDraw(int debugDrawFlags) { if (m_dynamicsWorld) + { + if (m_dynamicsWorld->getDebugDrawer()) + { + m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + } m_dynamicsWorld->debugDrawWorld(); + } } diff --git a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h index c9e069c2a..6bf152d1d 100644 --- a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h +++ b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h @@ -25,7 +25,7 @@ struct LuaPhysicsSetup : public CommonPhysicsSetup virtual void stepSimulation(float deltaTime); - virtual void debugDraw(); + virtual void debugDraw(int debugDrawFlags); virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp index a93b88e12..db5c98eef 100644 --- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp +++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp @@ -226,9 +226,9 @@ void Bullet2RigidBodyDemo::renderScene() } -void Bullet2RigidBodyDemo::physicsDebugDraw() +void Bullet2RigidBodyDemo::physicsDebugDraw(int debugDrawFlags) { - m_physicsSetup->debugDraw(); + m_physicsSetup->debugDraw(debugDrawFlags); } Bullet2RigidBodyDemo::~Bullet2RigidBodyDemo() diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h index 84c1dd405..5c881c33c 100644 --- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h +++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h @@ -25,7 +25,7 @@ public: virtual void initPhysics(); virtual void exitPhysics(); virtual void renderScene(); - virtual void physicsDebugDraw(); + virtual void physicsDebugDraw(int debugDrawFlags); virtual void stepSimulation(float dt); virtual CommonPhysicsSetup* getPhysicsSetup() { diff --git a/btgui/Bullet3AppSupport/BulletDemoInterface.h b/btgui/Bullet3AppSupport/BulletDemoInterface.h index 92bc8a8b7..41e6169c9 100644 --- a/btgui/Bullet3AppSupport/BulletDemoInterface.h +++ b/btgui/Bullet3AppSupport/BulletDemoInterface.h @@ -3,6 +3,7 @@ struct CommonGraphicsApp; + class BulletDemoInterface { public: @@ -17,7 +18,7 @@ public: virtual void exitPhysics()=0; virtual void stepSimulation(float deltaTime)=0; virtual void renderScene()=0; - virtual void physicsDebugDraw()=0; + virtual void physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h virtual bool mouseMoveCallback(float x,float y)=0; virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; virtual bool keyboardCallback(int key, int state)=0; @@ -44,7 +45,7 @@ public: virtual void renderScene() { } - virtual void physicsDebugDraw() + virtual void physicsDebugDraw(int debugFlags) { } virtual bool mouseMoveCallback(float x,float y) diff --git a/btgui/Bullet3AppSupport/CommonMultiBodySetup.h b/btgui/Bullet3AppSupport/CommonMultiBodySetup.h index 03553e3c2..53fc8156e 100644 --- a/btgui/Bullet3AppSupport/CommonMultiBodySetup.h +++ b/btgui/Bullet3AppSupport/CommonMultiBodySetup.h @@ -125,10 +125,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup } } - virtual void debugDraw() + virtual void debugDraw(int debugDrawFlags) { if (m_dynamicsWorld) { + if (m_dynamicsWorld->getDebugDrawer()) + { + m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + } m_dynamicsWorld->debugDrawWorld(); } diff --git a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h index 1e0f4e46e..0039eb1b2 100644 --- a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h +++ b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h @@ -57,7 +57,7 @@ public: virtual void stepSimulation(float deltaTime)=0; - virtual void debugDraw()=0; + virtual void debugDraw(int debugDrawFlags)=0; virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0; virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; diff --git a/btgui/Bullet3AppSupport/CommonRigidBodySetup.h b/btgui/Bullet3AppSupport/CommonRigidBodySetup.h index 41507db05..239bd13f3 100644 --- a/btgui/Bullet3AppSupport/CommonRigidBodySetup.h +++ b/btgui/Bullet3AppSupport/CommonRigidBodySetup.h @@ -118,10 +118,14 @@ struct CommonRigidBodySetup : public CommonPhysicsSetup } } - virtual void debugDraw() + virtual void debugDraw(int debugDrawFlags) { if (m_dynamicsWorld) { + if (m_dynamicsWorld->getDebugDrawer()) + { + m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + } m_dynamicsWorld->debugDrawWorld(); } diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 58aed9646..01f4c2ec1 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -1091,8 +1091,8 @@ void GLInstancingRenderer::updateCamera(int upAxis) }; - float m_frustumZNear=1; - float m_frustumZFar=10000.f; + float m_frustumZNear=0.1; + float m_frustumZFar=1000.f; // m_azi=m_azi+0.01; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index f80f2a5d3..361a054ec 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -34,6 +34,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" @@ -1306,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) } } break; + ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage + case D6_SPRING_2_CONSTRAINT_TYPE: + { + { + btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if (minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false); + } + else if (minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0)); + } + } + break; + } case SLIDER_CONSTRAINT_TYPE: { btSliderConstraint* pSlider = (btSliderConstraint*)constraint; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d8a34b7da..dd3d1c366 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -151,7 +151,7 @@ public: virtual void removeCollisionObject(btCollisionObject* collisionObject); - void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawConstraint(btTypedConstraint* constraint); virtual void debugDrawWorld(); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 2e930191e..dc51b0adf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisComOffset, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { btAssert(m_isMultiDof); m_links[i].m_mass = mass; - m_links[i].m_inertia = inertia; + m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].m_eVector = parentComToThisComOffset; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_jointType = btMultibodyLink::eFixed; m_links[i].m_dofCount = 0; @@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i, m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // m_links[i].updateCacheMultiDof(); + // //if(m_isMultiDof) // resizeInternalMultiDofBuffers(); @@ -180,12 +183,12 @@ void btMultiBody::setupPrismatic(int i, } m_links[i].m_mass = mass; - m_links[i].m_inertia = inertia; + m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].setAxisTop(0, 0., 0., 0.); m_links[i].setAxisBottom(0, jointAxis); - m_links[i].m_eVector = parentComToThisComOffset; + m_links[i].m_eVector = parentComToThisComOffset; m_links[i].m_cachedRotParentToThis = rotParentToThis; m_links[i].m_jointType = btMultibodyLink::ePrismatic; @@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i, } m_links[i].m_mass = mass; - m_links[i].m_inertia = inertia; + m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].setAxisTop(0, jointAxis); @@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i, m_posVarCnt += 4; m_links[i].m_mass = mass; - m_links[i].m_inertia = inertia; + m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].m_dVector = thisPivotToThisComOffset; @@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i, m_posVarCnt += 3; m_links[i].m_mass = mass; - m_links[i].m_inertia = inertia; + m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].m_dVector.setZero(); @@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const const btVector3 & btMultiBody::getLinkInertia(int i) const { - return m_links[i].m_inertia; + return m_links[i].m_inertiaLocal; } btScalar btMultiBody::getJointPos(int i) const @@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const for (int i = 0; i < num_links; ++i) { result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]); + result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); } return 0.5f * result; @@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const for (int i = 0; i < num_links; ++i) { rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1]))); + result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); } return result; @@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // //adding damping terms (only) btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), + zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); // calculate Ihat_i^A @@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, 0, m_links[i].m_mass, 0, 0, 0, m_links[i].m_mass), // - btMatrix3x3(m_links[i].m_inertia[0], 0, 0, - 0, m_links[i].m_inertia[1], 0, - 0, 0, m_links[i].m_inertia[2]) + btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, + 0, m_links[i].m_inertiaLocal[1], 0, + 0, 0, m_links[i].m_inertiaLocal[2]) ); // //p += vhat x Ihat vhat - done in a simpler way @@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt, - (rot_from_world[i+1] * m_links[i].m_appliedTorque); if (m_useGyroTerm) { - zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] ); + zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] ); } - zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); + zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); // calculate Ihat_i^A inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0, 0, m_links[i].m_mass, 0, 0, 0, m_links[i].m_mass); - inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0, - 0, m_links[i].m_inertia[1], 0, - 0, 0, m_links[i].m_inertia[2]); + inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0, + 0, m_links[i].m_inertiaLocal[1], 0, + 0, 0, m_links[i].m_inertiaLocal[2]); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 97825d7b4..c837f00c4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -61,7 +61,8 @@ public: const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisComOffset, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool disableParentCollision); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index af280d24a..951808fad 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include "LinearMath/btQuickprof.h" #include "btMultiBodyConstraint.h" - +#include "LinearMath/btIDebugDraw.h" @@ -755,3 +755,81 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* { m_multiBodyConstraints.remove(constraint); } + + +void btMultiBodyDynamicsWorld::debugDrawWorld() +{ + BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); + + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + + if (drawConstraints) + { + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + + for (int b = 0; bgetNumLinks(); + + ///base + num m_links + world_to_local.resize(nLinks + 1); + local_origin.resize(nLinks + 1); + + + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + + + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() }; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getDebugDrawer()->drawTransform(tr, 0.1); + + } + + for (int k = 0; kgetNumLinks(); k++) + { + const int parent = bod->getParent(k); + world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k))); + } + + + for (int m = 0; mgetNumLinks(); m++) + { + int link = m; + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() }; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getDebugDrawer()->drawTransform(tr, 0.1); + } + } + } + } + + btDiscreteDynamicsWorld::debugDrawWorld(); +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 79b7758b1..c519083bf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -54,5 +54,7 @@ public: virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); + + virtual void debugDrawWorld(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index a793f510e..c3cbaf9fd 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -355,7 +355,7 @@ struct btMultibodyLink BT_DECLARE_ALIGNED_ALLOCATOR(); btScalar m_mass; // mass of link - btVector3 m_inertia; // inertia of link (local frame; diagonal) + btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. @@ -440,15 +440,15 @@ struct btMultibodyLink btMultibodyLink() : m_mass(1), m_parent(-1), - m_zeroRotParentToThis(1, 0, 0, 0), - m_cachedRotParentToThis(1, 0, 0, 0), + m_zeroRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis(0, 0, 0, 1), m_collider(0), m_flags(0), m_dofCount(0), m_posVarCount(0), m_jointType(btMultibodyLink::eInvalid) { - m_inertia.setValue(1, 1, 1); + m_inertiaLocal.setValue(1, 1, 1); setAxisTop(0, 0., 0., 0.); setAxisBottom(0, 1., 0., 0.); m_dVector.setValue(0, 0, 0); @@ -493,7 +493,7 @@ struct btMultibodyLink case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0); + m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); break; } @@ -516,7 +516,7 @@ struct btMultibodyLink case eFixed: { m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector); + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); break; } diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index b913e1123..bdda0291a 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -43,6 +43,11 @@ static btClock gProfileClock; #include #else //_XBOX #include + +#if WINVER < 0x0600 +ULONGLONG GetTickCount64() { return GetTickCount(); } +#endif + #endif //_XBOX #include