From 5d40d90bd039150084862fe4bf1094fdca7efb92 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 3 Mar 2015 13:24:06 -0800 Subject: [PATCH] add btMultiBodyConstraint::finalizeMultiDof API: if you add multi-body constraints to a multi-dof btMultiBody, before it has been finalized using the btMultiBody::finalizeMultiDof call, then you have to manually call the btMultiBodyConstraint::finalizeMultiDof for all multi-dof multi body constraints. --- Demos/Raytracer/RaytracerSetup.cpp | 318 ++++++++++++++++++ Demos/Raytracer/RaytracerSetup.h | 42 +++ Demos3/AllBullet2Demos/BulletDemoEntries.h | 7 +- Demos3/AllBullet2Demos/main.cpp | 69 +++- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 84 ++++- Demos3/ImportURDFDemo/ImportURDFSetup.h | 1 + .../Bullet2RigidBodyDemo.cpp | 4 + .../Common2dCanvasInterface.h | 15 + btgui/Bullet3AppSupport/CommonPhysicsSetup.h | 5 + btgui/OpenGLWindow/CommonGraphicsApp.h | 4 +- .../Featherstone/btMultiBody.cpp | 2 +- .../Featherstone/btMultiBodyConstraint.cpp | 22 +- .../Featherstone/btMultiBodyConstraint.h | 2 +- 13 files changed, 548 insertions(+), 27 deletions(-) create mode 100644 Demos/Raytracer/RaytracerSetup.cpp create mode 100644 Demos/Raytracer/RaytracerSetup.h create mode 100644 btgui/Bullet3AppSupport/Common2dCanvasInterface.h diff --git a/Demos/Raytracer/RaytracerSetup.cpp b/Demos/Raytracer/RaytracerSetup.cpp new file mode 100644 index 000000000..6855ffc43 --- /dev/null +++ b/Demos/Raytracer/RaytracerSetup.cpp @@ -0,0 +1,318 @@ + +#include "RaytracerSetup.h" +#include "Bullet3AppSupport/Common2dCanvasInterface.h" +//#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +//#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +//#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" + +struct RaytracerInternalData +{ + int m_canvasIndex; + struct Common2dCanvasInterface* m_canvas; + + int m_width; + int m_height; + + btAlignedObjectArray m_shapePtr; + btAlignedObjectArray m_transforms; + btVoronoiSimplexSolver m_simplexSolver; + btScalar m_pitch; + btScalar m_roll; + btScalar m_yaw; + + RaytracerInternalData() + :m_canvasIndex(-1), + m_canvas(0), + m_roll(0), + m_pitch(0), + m_yaw(0), + m_width(128), + m_height(128) + { + btConeShape* cone = new btConeShape(1,1); + btSphereShape* sphere = new btSphereShape(1); + btBoxShape* box = new btBoxShape (btVector3(1,1,1)); + m_shapePtr.push_back(cone); + m_shapePtr.push_back(sphere); + m_shapePtr.push_back(box); + + updateTransforms(); + } + void updateTransforms() + { + int numObjects = m_shapePtr.size(); + m_transforms.resize(numObjects); + for (int i=0;im_canvas = gfxBridge.get2dCanvasInterface(); + + + if (m_internalData->m_canvas) + { + + m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height); + for (int i=0;im_width;i++) + { + for (int j=0;jm_height;j++) + { + unsigned char red=255; + unsigned char green=255; + unsigned char blue=255; + unsigned char alpha=255; + m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,i,j,red,green,blue,alpha); + } + } + m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex); + + //int bitmapId = gfxBridge.createRenderBitmap(width,height); + } + + + + +} + + +///worldRaytest performs a ray versus all objects in a collision world, returning true is a hit is found (filling in worldNormal and worldHitPoint) +bool RaytracerPhysicsSetup::worldRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint) +{ + return false; +} + + +///singleObjectRaytest performs a ray versus one collision shape, returning true is a hit is found (filling in worldNormal and worldHitPoint) +bool RaytracerPhysicsSetup::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint) +{ + return false; +} + + +///lowlevelRaytest performs a ray versus convex shape, returning true is a hit is found (filling in worldNormal and worldHitPoint) +bool RaytracerPhysicsSetup::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint) +{ + btScalar closestHitResults = 1.f; + + bool hasHit = false; + btConvexCast::CastResult rayResult; + btSphereShape pointShape(0.0f); + btTransform rayFromTrans; + btTransform rayToTrans; + + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFrom); + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayTo); + + int numObjects = m_internalData->m_shapePtr.size(); + + for (int s=0;sm_shapePtr[s]->getAabb( m_internalData->m_transforms[s],aabbMin,aabbMax); + btScalar hitLambda = 1.f; + btVector3 hitNormal; + btCollisionObject tmpObj; + tmpObj.setWorldTransform( m_internalData->m_transforms[s]); + + + if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal)) + { + //reset previous result + + //choose the continuous collision detection method + btSubsimplexConvexCast convexCaster(&pointShape, m_internalData->m_shapePtr[s],&m_internalData->m_simplexSolver); + //btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); + //btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0); + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans, m_internalData->m_transforms[s], m_internalData->m_transforms[s],rayResult)) + { + if (rayResult.m_fraction < closestHitResults) + { + closestHitResults = rayResult.m_fraction; + + worldNormal = m_internalData->m_transforms[s].getBasis() *rayResult.m_normal; + worldNormal.normalize(); + hasHit = true; + } + } + } + } + + + return hasHit; + +} + +void RaytracerPhysicsSetup::exitPhysics() +{ + + if (m_internalData->m_canvas && m_internalData->m_canvasIndex>=0) + { + m_internalData->m_canvas->destroyCanvas(m_internalData->m_canvasIndex); + } +} + +void RaytracerPhysicsSetup::stepSimulation(float deltaTime) +{ + + m_internalData->updateTransforms(); + + + float top = 1.f; + float bottom = -1.f; + float nearPlane = 1.f; + + float tanFov = (top-bottom)*0.5f / nearPlane; + + float fov = 2.0 * atanf (tanFov); + + btVector3 cameraPosition(5,0,0); + btVector3 cameraTargetPosition(0,0,0); + + btVector3 rayFrom = cameraPosition; + btVector3 rayForward = cameraTargetPosition-cameraPosition; + rayForward.normalize(); + float farPlane = 600.f; + rayForward*= farPlane; + + btVector3 rightOffset; + btVector3 vertical(0.f,1.f,0.f); + btVector3 hor; + hor = rayForward.cross(vertical); + hor.normalize(); + vertical = hor.cross(rayForward); + vertical.normalize(); + + float tanfov = tanf(0.5f*fov); + + hor *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + + btVector3 rayToCenter = rayFrom + rayForward; + + btVector3 dHor = hor * 1.f/float(m_internalData->m_width); + btVector3 dVert = vertical * 1.f/float(m_internalData->m_height); + + + + + int mode = 0; + int x,y; + + for (x=0;xm_width;x++) + { + for (int y=0;ym_height;y++) + { + btVector4 rgba(0,0,0,0); + btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; + rayTo += x * dHor; + rayTo -= y * dVert; + btVector3 worldNormal(0,0,0); + btVector3 worldPoint(0,0,0); + + + + bool hasHit = false; + int mode = 0; + switch (mode) + { + case 0: + hasHit = lowlevelRaytest(rayFrom,rayTo,worldNormal,worldPoint); + break; + case 1: + hasHit = singleObjectRaytest(rayFrom,rayTo,worldNormal,worldPoint); + break; + case 2: + hasHit = worldRaytest(rayFrom,rayTo,worldNormal,worldPoint); + break; + default: + { + } + } + + if (hasHit) + { + float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f)); + float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f)); + + + rgba = btVector4(lightVec0,lightVec1,0,1.f); + rgba.setMin(btVector3(1,1,1)); + rgba.setMax(btVector3(0.2,0.2,0.2)); + rgba[3] = 1.f; + unsigned char red = rgba[0] * 255; + unsigned char green = rgba[1] * 255; + unsigned char blue = rgba[2] * 255; + unsigned char alpha=255; + m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,y,red,green,blue,alpha); + + } else + { + // btVector4 rgba = raytracePicture->getPixel(x,y); + } + if (!rgba.length2()) + { + m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,y,255,0,0,255); + } + } + } + m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex); +} + + +void RaytracerPhysicsSetup::debugDraw(int debugDrawFlags) +{ +} + +bool RaytracerPhysicsSetup::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + return false; +} +bool RaytracerPhysicsSetup::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + return false; +} +void RaytracerPhysicsSetup::removePickingConstraint() +{ +} + + +void RaytracerPhysicsSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge) +{ +} \ No newline at end of file diff --git a/Demos/Raytracer/RaytracerSetup.h b/Demos/Raytracer/RaytracerSetup.h new file mode 100644 index 000000000..b7683d410 --- /dev/null +++ b/Demos/Raytracer/RaytracerSetup.h @@ -0,0 +1,42 @@ +#ifndef RAYTRACER_SETUP_H +#define RAYTRACER_SETUP_H + + +#include "Bullet3AppSupport/CommonRigidBodySetup.h" + +struct RaytracerPhysicsSetup : public CommonPhysicsSetup +{ + + struct RaytracerInternalData* m_internalData; + + RaytracerPhysicsSetup(); + + virtual ~RaytracerPhysicsSetup(); + + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + + virtual void exitPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void debugDraw(int debugDrawFlags); + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual void removePickingConstraint(); + + virtual void syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge); + + ///worldRaytest performs a ray versus all objects in a collision world, returning true is a hit is found (filling in worldNormal and worldHitPoint) + bool worldRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint); + + ///singleObjectRaytest performs a ray versus one collision shape, returning true is a hit is found (filling in worldNormal and worldHitPoint) + bool singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint); + + ///lowlevelRaytest performs a ray versus convex shape, returning true is a hit is found (filling in worldNormal and worldHitPoint) + bool lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint); + + +}; + +#endif //RAYTRACER_SETUP_H diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index c053eb2f8..ebda8397c 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -22,7 +22,7 @@ #include "../ImportObjDemo/ImportObjSetup.h" #include "../ImportSTLDemo/ImportSTLSetup.h" #include "../ImportColladaDemo/ImportColladaSetup.h" - +#include "../Demos/Raytracer/RaytracerSetup.h" #include "../../Demos/SerializeDemo/SerializeSetup.h" #include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h" #include "../bullet2/MultiBodyDemo/MultiBodyVehicle.h" @@ -62,6 +62,9 @@ MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup); MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup); MYCREATEFUNC(CoordinateFrameDemoPhysics); +MYCREATEFUNC(RaytracerPhysics); +//Bullet2RigidBodyDemo + static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app) { @@ -88,7 +91,7 @@ static BulletDemoEntry allDemos[]= {1,"SupportFunc", &MySupportFuncDemo::CreateFunc}, {0,"API Demos", 0}, - + {1,"Raytracer",RaytracerPhysicsCreateFunc}, {1,"BasicDemo",BasicDemo::MyCreateFunc}, { 1, "CcdDemo", CcdPhysicsCreateFunc }, { 1, "Kinematic", KinematicObjectCreateFunc }, diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index e5b8c86a7..42d075a9c 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -31,6 +31,7 @@ #include "Bullet3AppSupport/GwenProfileWindow.h" #include "Bullet3AppSupport/GwenTextureWindow.h" #include "Bullet3AppSupport/GraphingTexture.h" +#include "Bullet3AppSupport/Common2dCanvasInterface.h" #include "OpenGLWindow/SimpleCamera.h" #include "OpenGLWindow/SimpleOpenGL2Renderer.h" @@ -393,6 +394,71 @@ void fileOpenCallback() } } +#define MAX_GRAPH_WINDOWS 5 + +struct QuickCanvas : public Common2dCanvasInterface +{ + GL3TexLoader* m_myTexLoader; + + MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS]; + GraphingTexture* m_gt[MAX_GRAPH_WINDOWS]; + int m_curNumGraphWindows; + + QuickCanvas(GL3TexLoader* myTexLoader) + :m_myTexLoader(myTexLoader), + m_curNumGraphWindows(0) + { + for (int i=0;igetInternalData()); + input.m_width=width; + input.m_height=height; + input.m_xPos = 300; + input.m_yPos = height-input.m_height; + input.m_name=canvasName; + input.m_texName = canvasName; + m_gt[slot] = new GraphingTexture; + m_gt[slot]->create(width,height); + int texId = m_gt[slot]->getTextureId(); + m_myTexLoader->m_hashMap.insert(canvasName, texId); + m_gw[slot] = setupTextureWindow(input); + + return slot; + } + return -1; + } + virtual void destroyCanvas(int canvasId) + { + btAssert(canvasId==0);//hardcoded to zero for now, only a single canvas + btAssert(m_curNumGraphWindows==1); + destroyTextureWindow(m_gw[canvasId]); + m_curNumGraphWindows--; + } + virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha) + { + btAssert(canvasId==0);//hardcoded + btAssert(m_curNumGraphWindows==1); + m_gt[canvasId]->setPixel(x,y,red,green,blue,alpha); + } + virtual void refreshImageData(int canvasId) + { + m_gt[canvasId]->uploadImageData(); + } +}; + extern float shadowMapWorldSize; int main(int argc, char* argv[]) { @@ -522,7 +588,8 @@ int main(int argc, char* argv[]) //destroyTextureWindow(gw); #endif s_parameterInterface = app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData()); - + app->m_2dCanvasInterface = new QuickCanvas(myTexLoader); + //gui->getInternalData()->m_demoPage; int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry); diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index de9754a1c..814cb48a8 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -5,6 +5,7 @@ #include "../ImportSTLDemo/LoadMeshFromSTL.h" #include "../ImportColladaDemo/LoadMeshFromCollada.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "Bullet3Common/b3FileUtils.h" #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape @@ -16,10 +17,31 @@ static bool enableConstraints = true;//false; const char* fileNames[] = { "r2d2.urdf", + "r2d2.urdf", + }; +#define MAX_NUM_MOTORS 1024 + +struct ImportUrdfInternalData +{ + ImportUrdfInternalData() + :m_numMotors(0) + { + } + + float m_motorTargetVelocities[MAX_NUM_MOTORS]; + btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS]; + int m_numMotors; +}; + + + + ImportUrdfSetup::ImportUrdfSetup() { + m_data = new ImportUrdfInternalData; + static int count = 0; sprintf(m_fileName,fileNames[count++]); @@ -32,7 +54,7 @@ ImportUrdfSetup::ImportUrdfSetup() ImportUrdfSetup::~ImportUrdfSetup() { - + delete m_data; } static btVector4 colors[4] = @@ -684,7 +706,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha } return shape; } -void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) +void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix, ImportUrdfInternalData* data) { //btCollisionShape* shape = 0; @@ -952,6 +974,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { if (mappings.m_createMultiBody) { + printf("Revolute joint (btMultiBody)\n"); //todo: adjust the center of mass transform and pivot axis properly /*mappings.m_bulletMultiBody->setupRevolute( linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, @@ -969,11 +992,35 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy -offsetInB.getOrigin(), disableParentCollision); //linkInfo->m_localVisualFrame.setIdentity(); + + { + if (data->m_numMotorsname.c_str(); + char motorName[1024]; + sprintf(motorName,"%s q'", jointName); + + float* motorVel = &data->m_motorTargetVelocities[data->m_numMotors]; + *motorVel = 0.f; + + SliderParams slider(motorName,motorVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); + float maxMotorImpulse = 0.1f; + + btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mappings.m_bulletMultiBody,linkIndex-1,0,0,maxMotorImpulse); + data->m_jointMotors[data->m_numMotors]=motor; + world1->addMultiBodyConstraint(motor); + data->m_numMotors++; + } + } } else { //only handle principle axis at the moment, //@todo(erwincoumans) orient the constraint for non-principal axis + printf("Hinge joint (btGeneric6DofSpring2Constraint)\n"); btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z); int principleAxis = axis.closestAxis(); switch (principleAxis) @@ -1026,11 +1073,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { if (mappings.m_createMultiBody) { - //mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision); - - //mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision); + printf("Prismatic joint (btMultiBody)\n"); mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(), @@ -1038,10 +1081,12 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy disableParentCollision); + } else { + printf("Slider joint (btGeneric6DofSpring2Constraint)\n"); btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB); //todo(erwincoumans) for now, we only support principle axis along X, Y or Z btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z); @@ -1145,7 +1190,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { if (*child) { - URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix); + URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix, data); } else @@ -1236,18 +1281,26 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) int numJoints = (*robot).m_numJoints; - static bool useFeatherstone = false; + static bool useFeatherstone = true; { URDF2BulletMappings mappings; mappings.m_createMultiBody = useFeatherstone; mappings.m_totalNumJoints = numJoints; - URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix); + URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix,m_data); if (useFeatherstone) { btMultiBody* mb = mappings.m_bulletMultiBody; mb->setHasSelfCollision(false); - mb->finalizeMultiDof(); + if (mb->isMultiDof()) + { + mb->finalizeMultiDof(); + } m_dynamicsWorld->addMultiBody(mb); + + for (int i=0;im_numMotors;i++) + { + m_data->m_jointMotors[i]->finalizeMultiDof(); + } } } @@ -1284,7 +1337,14 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { if (m_dynamicsWorld) { + //set the new target velocities + for (int i=0;im_numMotors;i++) + { + m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]); + } //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge - m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + int actualSteps = m_dynamicsWorld->stepSimulation(1./240.,0);//deltaTime,10,1./240.); + //int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,1000,1./3000.f);//240.); + printf("deltaTime %f took actualSteps = %d\n",deltaTime,actualSteps); } } diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.h b/Demos3/ImportURDFDemo/ImportURDFSetup.h index 482a36915..4d848b4e0 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.h +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.h @@ -7,6 +7,7 @@ class ImportUrdfSetup : public CommonMultiBodySetup { char m_fileName[1024]; + struct ImportUrdfInternalData* m_data; public: ImportUrdfSetup(); diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp index a991023fe..e68b95ba2 100644 --- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp +++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp @@ -180,6 +180,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge } + virtual struct Common2dCanvasInterface* get2dCanvasInterface() + { + return m_glApp->m_2dCanvasInterface; + } virtual CommonParameterInterface* getParameterInterface() { return m_glApp->m_parameterInterface; diff --git a/btgui/Bullet3AppSupport/Common2dCanvasInterface.h b/btgui/Bullet3AppSupport/Common2dCanvasInterface.h new file mode 100644 index 000000000..3fcfc40f2 --- /dev/null +++ b/btgui/Bullet3AppSupport/Common2dCanvasInterface.h @@ -0,0 +1,15 @@ +#ifndef COMMON_2D_CANVAS_INTERFACE_H +#define COMMON_2D_CANVAS_INTERFACE_H + +struct Common2dCanvasInterface +{ + virtual ~Common2dCanvasInterface() {} + virtual int createCanvas(const char* canvasName, int width, int height)=0; + virtual void destroyCanvas(int canvasId)=0; + virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0; + virtual void refreshImageData(int canvasId)=0; + +}; + +#endif //COMMON_2D_CANVAS_INTERFACE_H + diff --git a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h index 76b803d24..dbd2801f2 100644 --- a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h +++ b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h @@ -38,6 +38,10 @@ struct GraphicsPhysicsBridge virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;} + virtual struct Common2dCanvasInterface* get2dCanvasInterface() + { + return 0; + } virtual CommonParameterInterface* getParameterInterface() { @@ -65,6 +69,7 @@ public: 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; virtual void removePickingConstraint() = 0; diff --git a/btgui/OpenGLWindow/CommonGraphicsApp.h b/btgui/OpenGLWindow/CommonGraphicsApp.h index d69a6bc48..4a722c31d 100644 --- a/btgui/OpenGLWindow/CommonGraphicsApp.h +++ b/btgui/OpenGLWindow/CommonGraphicsApp.h @@ -25,7 +25,8 @@ struct CommonGraphicsApp CommonGraphicsApp() :m_window(0), m_renderer(0), - m_parameterInterface(0) + m_parameterInterface(0), + m_2dCanvasInterface(0) { } virtual ~CommonGraphicsApp() @@ -35,6 +36,7 @@ struct CommonGraphicsApp class b3gWindowInterface* m_window; struct CommonRenderInterface* m_renderer; struct CommonParameterInterface* m_parameterInterface; + struct Common2dCanvasInterface* m_2dCanvasInterface; virtual void drawGrid(DrawGridData data=DrawGridData()) = 0; virtual void setUpAxis(int axis) = 0; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 40d8ceaa1..5b65864ae 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2171,7 +2171,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Y_i (scratch), invD_i (cached) const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; - btScalar * Y = r_ptr; + btScalar * Y = r_ptr; //////////////// //aux variables static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d779e926e..2cf0ff8d8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -15,21 +15,25 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_isUnilateral(isUnilateral), m_maxAppliedImpulse(100) { - - if(bodyA) + finalizeMultiDof(); +} + +void btMultiBodyConstraint::finalizeMultiDof() +{ + if(m_bodyA) { - if(bodyA->isMultiDof()) - m_jacSizeA = (6 + bodyA->getNumDofs()); + if(m_bodyA->isMultiDof()) + m_jacSizeA = (6 + m_bodyA->getNumDofs()); else - m_jacSizeA = (6 + bodyA->getNumLinks()); + m_jacSizeA = (6 + m_bodyA->getNumLinks()); } - if(bodyB) + if(m_bodyB) { - if(bodyB->isMultiDof()) - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); + if(m_bodyB->isMultiDof()) + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); else - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks(); } else m_jacSizeBoth = m_jacSizeA; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 93868ceeb..2a96ea85b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -82,7 +82,7 @@ public: btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); virtual ~btMultiBodyConstraint(); - + void finalizeMultiDof(); virtual int getIslandIdA() const =0; virtual int getIslandIdB() const =0;