From a6bf3d023edea7535fe72679374834d09fee6c2b Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 16 Dec 2014 17:20:37 -0800 Subject: [PATCH 1/6] update interfaces for GL2 --- Demos3/AllBullet2Demos/main.cpp | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index e570e4ca8..a6f23f69b 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -8,7 +8,7 @@ #include "OpenGLWindow/SimpleOpenGL3App.h" #endif - +#include "OpenGLWindow/CommonRenderInterface.h" #ifdef __APPLE__ #include "OpenGLWindow/MacOpenGLWindow.h" #else @@ -34,7 +34,7 @@ #include "Bullet3AppSupport/GwenProfileWindow.h" #include "Bullet3AppSupport/GwenTextureWindow.h" #include "Bullet3AppSupport/GraphingTexture.h" -#include "OpenGLWindow/CommonRenderInterface.h" + #include "OpenGLWindow/SimpleCamera.h" CommonGraphicsApp* app=0; @@ -140,6 +140,10 @@ struct TestRenderer : public CommonRenderInterface { return m_height; } + virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling) + { + return 0; + } virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return 0; @@ -192,6 +196,26 @@ struct TestRenderer : public CommonRenderInterface { } + + virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth) + { + + } + virtual void drawPoint(const float* position, const float color[4], float pointDrawSize) + { + } + virtual void drawPoint(const double* position, const double color[4], double pointDrawSize) + { + } + + virtual void updateShape(int shapeIndex, const float* vertices) + { + } + + virtual void enableBlend(bool blend) + { + } + }; #endif //USE_OPENGL2 b3gWindowInterface* s_window = 0; From 6e70f285e1ccc0eb845614a681c8d637a7af0ba3 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 22 Dec 2014 14:04:19 -0800 Subject: [PATCH 2/6] fix a 64-bit issue in the optional 'preSwapFilenameOut' method (this is only use when you want to export a copy of a .bullet file after loading with a different endian-ness) --- Extras/Serialize/BulletFileLoader/bFile.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Extras/Serialize/BulletFileLoader/bFile.cpp b/Extras/Serialize/BulletFileLoader/bFile.cpp index 8d1afbb12..e6ed57c48 100644 --- a/Extras/Serialize/BulletFileLoader/bFile.cpp +++ b/Extras/Serialize/BulletFileLoader/bFile.cpp @@ -388,7 +388,9 @@ void bFile::swapDNA(char* ptr) { bool swap = ((mFlags & FD_ENDIAN_SWAP)!=0); - char* data = &ptr[20]; + int offset = (mFlags & FD_FILE_64)? 24 : 20; + char* data = &ptr[offset]; + // void bDNA::init(char *data, int len, bool swap) int *intPtr=0;short *shtPtr=0; char *cp = 0;int dataLen =0;long nr=0; From a159fbac698520b7fcca585d6129eaa79bcdd69b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 22 Jan 2015 17:56:24 -0800 Subject: [PATCH 3/6] 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 From 51f41d0cfcf7638437e6c4e25d8d54c6ee9a2de4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 22 Jan 2015 18:28:00 -0800 Subject: [PATCH 4/6] fix issue, when debug drawer doesn't exist don't try to use it enable simulation by default (was accidently switched off at startup of AllBullet2Demos) --- Demos3/AllBullet2Demos/main.cpp | 2 +- .../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 41c7e0b19..4aa9e6ce6 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -235,7 +235,7 @@ static bool visualWireframe=false; static bool renderVisualGeometry=true; static bool renderGrid = true; int gDebugDrawFlags = 0; -static bool pauseSimulation=true; +static bool pauseSimulation=false; int midiBaseIndex = 176; extern bool gDisableDeactivation; diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index f652ba625..61f6a97c1 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -616,7 +616,8 @@ void FeatherstoneDemo1::physicsDebugDraw(int debugDrawFlags) { if (m_dynamicsWorld) { - m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); m_dynamicsWorld->debugDrawWorld(); } } From bd16df8dd6362c7b207c4c6300b86781dbd1f9ee Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 27 Jan 2015 10:45:56 -0800 Subject: [PATCH 5/6] add basic debug drawing interface for btMultiBodyPoint2Point constraint add basic debug drawing drawText3D in SimpleOpenGL3App remove a few warnings add drawTexturedRect3D to GLPrimitiveRenderer to support debug drawing --- .../BasicConcepts/CoordinateSystemDemo.h | 3 + .../CoordinateFrameDemoPhysicsSetup.cpp | 1 + btgui/OpenGLWindow/CMakeLists.txt | 11 +- btgui/OpenGLWindow/CommonGraphicsApp.h | 3 +- btgui/OpenGLWindow/CommonRenderInterface.h | 54 ++++ btgui/OpenGLWindow/GLInstancingRenderer.cpp | 107 ++++--- btgui/OpenGLWindow/GLInstancingRenderer.h | 8 +- btgui/OpenGLWindow/GLPrimInternalData.h | 2 + btgui/OpenGLWindow/GLPrimitiveRenderer.cpp | 117 ++++---- btgui/OpenGLWindow/GLPrimitiveRenderer.h | 34 +++ btgui/OpenGLWindow/MacOpenGLWindow.mm | 102 ++++--- btgui/OpenGLWindow/SimpleOpenGL3App.cpp | 269 ++++++++++++++++-- btgui/OpenGLWindow/SimpleOpenGL3App.h | 7 +- .../Featherstone/btMultiBodyConstraint.h | 1 + .../Featherstone/btMultiBodyDynamicsWorld.cpp | 15 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 2 + .../btMultiBodyJointLimitConstraint.h | 6 +- .../Featherstone/btMultiBodyJointMotor.h | 6 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 53 +++- .../Featherstone/btMultiBodyPoint2Point.h | 1 + 20 files changed, 611 insertions(+), 191 deletions(-) diff --git a/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h b/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h index f6aedb83b..7c772aebb 100644 --- a/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h +++ b/Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h @@ -60,6 +60,9 @@ public: virtual void renderScene() { m_app->m_renderer->renderScene(); + m_app->drawText3D("X",1,0,0,1); + m_app->drawText3D("Y",0,1,0,1); + m_app->drawText3D("Z",0,0,1,1); } virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, diff --git a/Demos3/bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp b/Demos3/bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp index a8f31b71c..48693b90a 100644 --- a/Demos3/bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp +++ b/Demos3/bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp @@ -21,6 +21,7 @@ void CoordinateFrameDemoPhysicsSetup::debugDraw() } } */ + m_dynamicsWorld->debugDrawWorld(); } diff --git a/btgui/OpenGLWindow/CMakeLists.txt b/btgui/OpenGLWindow/CMakeLists.txt index f222d1b21..57c476d8c 100644 --- a/btgui/OpenGLWindow/CMakeLists.txt +++ b/btgui/OpenGLWindow/CMakeLists.txt @@ -1,7 +1,6 @@ INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/src - ${BULLET_PHYSICS_SOURCE_DIR}/btgui + .. ) FILE(GLOB OpenGLWindow_HDRS "*.h" ) @@ -19,9 +18,9 @@ LIST(REMOVE_ITEM OpenGLWindowCommon_CPP X11OpenGLWindow.cpp ) #MESSAGE (${OpenGLWindowCommon_CPP}) IF (WIN32) - SET(OpenGLWindow_SRCS ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows/glew.c ${OpenGLWindowWin32_CPP} ${OpenGLWindowCommon_CPP}) + SET(OpenGLWindow_SRCS GlewWindows/glew.c ${OpenGLWindowWin32_CPP} ${OpenGLWindowCommon_CPP}) INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + GlewWindows ) ADD_DEFINITIONS(-DGLEW_STATIC) ENDIF(WIN32) @@ -33,13 +32,13 @@ ENDIF(APPLE) #no Linux detection? IF(NOT WIN32 AND NOT APPLE) INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + GlewWindows ) ADD_DEFINITIONS(-DGLEW_STATIC) ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") - SET(OpenGLWindow_SRCS ${OpenGLWindowLinux_CPP} ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows/glew.c ${OpenGLWindowCommon_CPP} ) + SET(OpenGLWindow_SRCS ${OpenGLWindowLinux_CPP} GlewWindows/glew.c ${OpenGLWindowCommon_CPP} ) ENDIF() diff --git a/btgui/OpenGLWindow/CommonGraphicsApp.h b/btgui/OpenGLWindow/CommonGraphicsApp.h index 953fdeca7..d69a6bc48 100644 --- a/btgui/OpenGLWindow/CommonGraphicsApp.h +++ b/btgui/OpenGLWindow/CommonGraphicsApp.h @@ -42,9 +42,10 @@ struct CommonGraphicsApp virtual void swapBuffer() = 0; virtual void drawText( const char* txt, int posX, int posY) = 0; - + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0; virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ)=0; virtual int registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10)=0; + virtual void registerGrid(int xres, int yres, float color0[4], float color1[4])=0; }; diff --git a/btgui/OpenGLWindow/CommonRenderInterface.h b/btgui/OpenGLWindow/CommonRenderInterface.h index 58bb42c23..e786672ef 100644 --- a/btgui/OpenGLWindow/CommonRenderInterface.h +++ b/btgui/OpenGLWindow/CommonRenderInterface.h @@ -32,6 +32,9 @@ struct CommonRenderInterface virtual void getCameraTargetPosition(float cameraPos[4]) const=0; virtual void getCameraTargetPosition(double cameraPos[4]) const=0; + virtual void getCameraViewMatrix(float viewMat[16]) const=0; + virtual void getCameraProjectionMatrix(float projMat[16]) const=0; + virtual void renderScene()=0; virtual int getScreenWidth() = 0; @@ -49,8 +52,59 @@ struct CommonRenderInterface virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0; virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0; + virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0; + virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0; virtual void writeTransforms()=0; virtual void enableBlend(bool blend)=0; }; + +template +inline int projectWorldCoordToScreen(T objx, T objy, T objz, + const T modelMatrix[16], + const T projMatrix[16], + const int viewport[4], + T *winx, T *winy, T *winz) +{ + int i; + T in2[4]; + T tmp[4]; + + in2[0]=objx; + in2[1]=objy; + in2[2]=objz; + in2[3]=T(1.0); + + for (i=0; i<4; i++) + { + tmp[i] = in2[0] * modelMatrix[0*4+i] + in2[1] * modelMatrix[1*4+i] + + in2[2] * modelMatrix[2*4+i] + in2[3] * modelMatrix[3*4+i]; + } + + T out[4]; + for (i=0; i<4; i++) + { + out[i] = tmp[0] * projMatrix[0*4+i] + tmp[1] * projMatrix[1*4+i] + tmp[2] * projMatrix[2*4+i] + tmp[3] * projMatrix[3*4+i]; + } + + if (out[3] == T(0.0)) + return 0; + out[0] /= out[3]; + out[1] /= out[3]; + out[2] /= out[3]; + /* Map x, y and z to range 0-1 */ + out[0] = out[0] * T(0.5) + T(0.5); + out[1] = out[1] * T(0.5) + T(0.5); + out[2] = out[2] * T(0.5) + T(0.5); + + /* Map x,y to viewport */ + out[0] = out[0] * viewport[2] + viewport[0]; + out[1] = out[1] * viewport[3] + viewport[1]; + + *winx=out[0]; + *winy=out[1]; + *winz=out[2]; + return 1; +} + #endif//COMMON_RENDER_INTERFACE_H diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 01f4c2ec1..3fbe9035b 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -112,8 +112,7 @@ struct b3GraphicsInstance bool m_ortho = false; -static GLfloat projectionMatrix[16]; -static GLfloat modelviewMatrix[16]; + //static GLfloat depthLightModelviewMatrix[16]; @@ -157,7 +156,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData int m_middleMouseButton; int m_rightMouseButton; - + GLfloat m_projectionMatrix[16]; + GLfloat m_viewMatrix[16]; GLuint m_defaultTexturehandle; @@ -185,7 +185,12 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData m_altPressed(0), m_controlPressed(0) { - + //clear to zero to make it obvious if the matrix is used uninitialized + for (int i=0;i<16;i++) + { + m_projectionMatrix[i]=0; + m_viewMatrix[i]=0; + } } @@ -224,6 +229,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData void mouseMoveCallback(float x, float y) { +// printf("moved to %f,%f\n",x,y); if (m_altPressed || m_controlPressed) { float xDelta = x-m_mouseXpos; @@ -460,6 +466,13 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi */ } +void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex) +{ + m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]); + m_data->m_instance_colors_ptr[srcIndex*4+1]=float(color[1]); + m_data->m_instance_colors_ptr[srcIndex*4+2]=float(color[2]); + m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]); +} void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIndex) { @@ -643,6 +656,7 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* m_data->m_instance_scale_ptr[index*3+2] = scaling[2]; gfxObj->m_numGraphicsInstances++; + m_data->m_totalNumInstances++; } else { b3Error("registerGraphicsInstance out of range, %d\n", maxElements); @@ -690,8 +704,6 @@ void GLInstancingRenderer::updateShape(int shapeIndex, const float* vertices) glUnmapBuffer( GL_ARRAY_BUFFER); } - - int GLInstancingRenderer::registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { b3GraphicsInstance* gfxObj = new b3GraphicsInstance; @@ -897,9 +909,9 @@ void GLInstancingRenderer::init() { if (x<2||y<2||x>253||y>253) { - pi[0]=0; - pi[1]=0; - pi[2]=0; + pi[0]=255;//0; + pi[1]=255;//0; + pi[2]=255;//0; } else { pi[0]=255; @@ -1091,7 +1103,7 @@ void GLInstancingRenderer::updateCamera(int upAxis) }; - float m_frustumZNear=0.1; + float m_frustumZNear=0.01; float m_frustumZFar=1000.f; @@ -1132,13 +1144,13 @@ void GLInstancingRenderer::updateCamera(int upAxis) if (m_screenWidth > m_screenHeight) { - b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix); + b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,m_data->m_projectionMatrix); } else { - b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix); + b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,m_data->m_projectionMatrix); } - b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,modelviewMatrix); + b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,m_data->m_viewMatrix); } @@ -1276,7 +1288,7 @@ void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName pixels[(j*textureWidth+i)*numComponents]=char(orgPixels[(j*textureWidth+i)]*255.f); pixels[(j*textureWidth+i)*numComponents+1]=0;//255.f; pixels[(j*textureWidth+i)*numComponents+2]=0;//255.f; - pixels[(j*textureWidth+i)*numComponents+3]=255; + pixels[(j*textureWidth+i)*numComponents+3]=127; //pixels[(j*textureWidth+i)*+1]=val; @@ -1342,8 +1354,8 @@ void GLInstancingRenderer::renderScene() void GLInstancingRenderer::drawPoint(const double* position, const double color[4], double pointDrawSize) { - float pos[4]={position[0],position[1],position[2],0}; - float clr[4] = {color[0],color[1],color[2],color[3]}; + float pos[4]={(float)position[0],(float)position[1],(float)position[2],0}; + float clr[4] = {(float)color[0],(float)color[1],(float)color[2],(float)color[3]}; drawPoints(pos,clr,1,3*sizeof(float),float(pointDrawSize)); } @@ -1351,16 +1363,16 @@ void GLInstancingRenderer::drawPoint(const float* positions, const float color[4 { drawPoints(positions,color,1,3*sizeof(float),pointDrawSize); } - void GLInstancingRenderer::drawPoints(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, float pointDrawSize) { glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D,0); - b3Assert(glGetError() ==GL_NO_ERROR);glUseProgram(linesShader); - glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]); + b3Assert(glGetError() ==GL_NO_ERROR); + glUseProgram(linesShader); + glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); glUniform4f(lines_colour,color[0],color[1],color[2],color[3]); glPointSize(pointDrawSize); @@ -1393,22 +1405,26 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[ glBindVertexArray(0); glPointSize(1); + glUseProgram(0); } -void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize) +void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn) { + float lineWidth = lineWidthIn; + b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]); + glLineWidth(lineWidth); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo); - b3Assert(glGetError() ==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D,0); - b3Assert(glGetError() ==GL_NO_ERROR);glUseProgram(linesShader); - glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]); + b3Assert(glGetError() ==GL_NO_ERROR); + glUseProgram(linesShader); + glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); glUniform4f(lines_colour,color[0],color[1],color[2],color[3]); // glPointSize(pointDrawSize); @@ -1450,7 +1466,7 @@ void GLInstancingRenderer::drawLines(const float* positions, const float color[4 b3Assert(glGetError() ==GL_NO_ERROR); glPointSize(1); b3Assert(glGetError() ==GL_NO_ERROR); - + glUseProgram(0); } void GLInstancingRenderer::drawLine(const double fromIn[4], const double toIn[4], const double colorIn[4], double lineWidthIn) @@ -1463,6 +1479,7 @@ void GLInstancingRenderer::drawLine(const double fromIn[4], const double toIn[4] } void GLInstancingRenderer::drawLine(const float from[4], const float to[4], const float color[4], float lineWidth) { + b3Assert(glGetError() ==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); @@ -1475,8 +1492,8 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons b3Assert(glGetError() ==GL_NO_ERROR); - glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]); + glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); glUniform4f(lines_colour,color[0],color[1],color[2],color[3]); @@ -1526,7 +1543,7 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons glLineWidth(1); b3Assert(glGetError() ==GL_NO_ERROR); - + glUseProgram(0); } struct PointerCaster @@ -1769,8 +1786,8 @@ b3Assert(glGetError() ==GL_NO_ERROR); if (gfxObj->m_primitiveType==B3_GL_POINTS) { glUseProgram(instancingShaderPointSprite); - glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &modelviewMatrix[0]); + glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]); glUniform1f(screenWidthPointSprite,float(m_screenWidth)); //glUniform1i(uniform_texture_diffusePointSprite, 0); @@ -1792,8 +1809,8 @@ b3Assert(glGetError() ==GL_NO_ERROR); case B3_DEFAULT_RENDERMODE: { glUseProgram(instancingShader); - glUniformMatrix4fv(ProjectionMatrix, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrix, 1, false, &modelviewMatrix[0]); + glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); glUniform1i(uniform_texture_diffuse, 0); glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); break; @@ -1821,10 +1838,10 @@ b3Assert(glGetError() ==GL_NO_ERROR); } glUseProgram(useShadowMapInstancingShader); - glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &projectionMatrix[0]); - glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &modelviewMatrix[0]); + glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); float MVP[16]; - b3Matrix4x4Mul16(projectionMatrix,modelviewMatrix,MVP); + b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP); glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]); b3Vector3 gLightDir = gLightPos; gLightDir.normalize(); @@ -1877,7 +1894,7 @@ b3Assert(glGetError() ==GL_NO_ERROR); - + glDisable(GL_CULL_FACE); b3Assert(glGetError() ==GL_NO_ERROR); } @@ -1894,3 +1911,19 @@ void GLInstancingRenderer::enableShadowMap() //glBindTexture(GL_TEXTURE_2D, m_data->m_defaultTexturehandle); } + +void GLInstancingRenderer::getCameraViewMatrix(float viewMat[16]) const +{ + for (int i=0;i<16;i++) + { + viewMat[i] = m_data->m_viewMatrix[i]; + } + +} +void GLInstancingRenderer::getCameraProjectionMatrix(float projMat[16]) const +{ + for (int i=0;i<16;i++) + { + projMat[i] = m_data->m_projectionMatrix[i]; + } +} diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.h b/btgui/OpenGLWindow/GLInstancingRenderer.h index 68880093d..e02b937d8 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.h +++ b/btgui/OpenGLWindow/GLInstancingRenderer.h @@ -64,8 +64,7 @@ public: ///vertices must be in the format x,y,z, nx,ny,nz, u,v virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1); - - + virtual int registerTexture(const unsigned char* texels, int width, int height); ///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z @@ -94,6 +93,7 @@ public: virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex); virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); + virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex); virtual void getMouseDirection(float* dir, int mouseX, int mouseY); @@ -141,6 +141,10 @@ public: virtual float getCameraYaw() const; virtual float getCameraPitch() const; + virtual void getCameraViewMatrix(float viewMat[16]) const; + virtual void getCameraProjectionMatrix(float projMat[16]) const; + + virtual void resize(int width, int height); virtual int getScreenWidth() { diff --git a/btgui/OpenGLWindow/GLPrimInternalData.h b/btgui/OpenGLWindow/GLPrimInternalData.h index 30b695396..5fdc62991 100644 --- a/btgui/OpenGLWindow/GLPrimInternalData.h +++ b/btgui/OpenGLWindow/GLPrimInternalData.h @@ -6,6 +6,8 @@ struct PrimInternalData { GLuint m_shaderProg; + GLint m_viewmatUniform; + GLint m_projMatUniform; GLint m_positionUniform; GLint m_colourAttribute; GLint m_positionAttribute; diff --git a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp index ceea63ee8..cff4bd52e 100644 --- a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -5,11 +5,10 @@ #include - -static const char* vertexShader= \ +static const char* vertexShader3D= \ "#version 150 \n" "\n" -"\n" +"uniform mat4 viewMatrix, projMatrix;\n" "in vec4 position;\n" "in vec4 colour;\n" "out vec4 colourV;\n" @@ -21,11 +20,11 @@ static const char* vertexShader= \ "void main (void)\n" "{\n" " colourV = colour;\n" -" gl_Position = vec4(position.x, position.y,0.f,1.f);\n" +" gl_Position = projMatrix * viewMatrix * position ;\n" " texuvV=texuv;\n" "}\n"; -static const char* fragmentShader= \ +static const char* fragmentShader3D= \ "#version 150\n" "\n" "uniform vec2 p;\n" @@ -42,42 +41,12 @@ static const char* fragmentShader= \ " {\n" " texcolor = vec4(1,1,1,texcolor.x);\n" " }\n" -"\n" -" fragColour = colourV*texcolor;\n" +" fragColour = colourV*texcolor;\n" "}\n"; + static unsigned int s_indexData[6] = {0,1,2,0,2,3}; -struct vec2 -{ - vec2(float x, float y) - { - p[0] = x; - p[1] = y; - } - float p[2]; -}; - -struct vec4 -{ - vec4(float x,float y, float z, float w) - { - p[0] = x; - p[1] = y; - p[2] = z; - p[3] = w; - - } - - float p[4]; -}; - -typedef struct -{ - vec4 position; - vec4 colour; - vec2 uv; -} Vertex; @@ -90,9 +59,16 @@ m_screenHeight(screenHeight) m_data = new PrimInternalData; - m_data->m_shaderProg = gltLoadShaderPair(vertexShader,fragmentShader); + m_data->m_shaderProg = gltLoadShaderPair(vertexShader3D,fragmentShader3D); - + m_data->m_viewmatUniform = glGetUniformLocation(m_data->m_shaderProg,"viewMatrix"); + if (m_data->m_viewmatUniform < 0) { + assert(0); + } + m_data->m_projMatUniform = glGetUniformLocation(m_data->m_shaderProg,"projMatrix"); + if (m_data->m_projMatUniform < 0) { + assert(0); + } m_data->m_positionUniform = glGetUniformLocation(m_data->m_shaderProg, "p"); if (m_data->m_positionUniform < 0) { assert(0); @@ -117,11 +93,11 @@ m_screenHeight(screenHeight) void GLPrimitiveRenderer::loadBufferData() { - Vertex vertexData[4] = { - { vec4(-1, -1, 0.0, 1.0 ), vec4( 1.0, 0.0, 0.0, 1.0 ) ,vec2(0,0)}, - { vec4(-1, 1, 0.0, 1.0 ), vec4( 0.0, 1.0, 0.0, 1.0 ) ,vec2(0,1)}, - { vec4( 1, 1, 0.0, 1.0 ), vec4( 0.0, 0.0, 1.0, 1.0 ) ,vec2(1,1)}, - { vec4( 1, -1, 0.0, 1.0 ), vec4( 1.0, 1.0, 1.0, 1.0 ) ,vec2(1,0)} + PrimVertex vertexData[4] = { + { PrimVec4(-1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 0.0, 0.0, 1.0 ) ,PrimVec2(0,0)}, + { PrimVec4(-1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 1.0, 0.0, 1.0 ) ,PrimVec2(0,1)}, + { PrimVec4( 1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 0.0, 1.0, 1.0 ) ,PrimVec2(1,1)}, + { PrimVec4( 1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 1.0, 1.0, 1.0 ) ,PrimVec2(1,0)} }; @@ -130,7 +106,7 @@ void GLPrimitiveRenderer::loadBufferData() glGenBuffers(1, &m_data->m_vertexBuffer); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); - glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(Vertex), vertexData, GL_DYNAMIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(PrimVertex), vertexData, GL_DYNAMIC_DRAW); assert(glGetError()==GL_NO_ERROR); @@ -146,9 +122,9 @@ void GLPrimitiveRenderer::loadBufferData() glEnableVertexAttribArray(m_data->m_textureAttribute); - glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); - glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); - glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); + glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0); + glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4)); + glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4))); assert(glGetError()==GL_NO_ERROR); @@ -225,13 +201,18 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float } -void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) + +void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA) { + assert(glGetError()==GL_NO_ERROR); glUseProgram(m_data->m_shaderProg); - + + glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, viewMat); + glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, projMat); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); @@ -248,14 +229,11 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); } - Vertex vertexData[4] = { - { vec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 0.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v0)}, - { vec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v1)}, - { vec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 1.f, 1.f ), vec4(color[0], color[1], color[2], color[3]) ,vec2(u1,v1)}, - { vec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 1.f, 0.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u1,v0)} - }; + PrimVertex vertexData[4] = { + v0,v1,v2,v3 + }; - glBufferSubData(GL_ARRAY_BUFFER, 0,4 * sizeof(Vertex), vertexData); + glBufferSubData(GL_ARRAY_BUFFER, 0,4 * sizeof(PrimVertex), vertexData); @@ -264,7 +242,7 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y assert(glGetError()==GL_NO_ERROR); - vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); + PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); if (useRGBA) { p.p[0] = 1.f; @@ -285,9 +263,9 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y glEnableVertexAttribArray(m_data->m_textureAttribute); - glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); - glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); - glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); + glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0); + glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4)); + glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4))); assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer); @@ -319,6 +297,23 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y } + +void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) +{ + float identity[16]={1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1}; + PrimVertex vertexData[4] = { + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + }; + + drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,useRGBA); +} + void GLPrimitiveRenderer::setScreenSize(int width, int height) { m_screenWidth = width; diff --git a/btgui/OpenGLWindow/GLPrimitiveRenderer.h b/btgui/OpenGLWindow/GLPrimitiveRenderer.h index 96fe17796..1c4f2e3f3 100644 --- a/btgui/OpenGLWindow/GLPrimitiveRenderer.h +++ b/btgui/OpenGLWindow/GLPrimitiveRenderer.h @@ -3,6 +3,39 @@ //#include "OpenGLInclude.h" +struct PrimVec2 +{ + PrimVec2(float x, float y) + { + p[0] = x; + p[1] = y; + } + float p[2]; +}; + +struct PrimVec4 +{ + PrimVec4() {} + PrimVec4(float x,float y, float z, float w) + { + p[0] = x; + p[1] = y; + p[2] = z; + p[3] = w; + + } + + float p[4]; +}; + +typedef struct +{ + PrimVec4 position; + PrimVec4 colour; + PrimVec2 uv; +} PrimVertex; + + class GLPrimitiveRenderer { int m_screenWidth; @@ -19,6 +52,7 @@ public: void drawRect(float x0, float y0, float x1, float y1, float color[4]); void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0); + void drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA = true); void drawLine();//float from[4], float to[4], float color[4]); void setScreenSize(int width, int height); diff --git a/btgui/OpenGLWindow/MacOpenGLWindow.mm b/btgui/OpenGLWindow/MacOpenGLWindow.mm index 6a593a091..50998b7e7 100644 --- a/btgui/OpenGLWindow/MacOpenGLWindow.mm +++ b/btgui/OpenGLWindow/MacOpenGLWindow.mm @@ -398,11 +398,12 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) // }]; //see http://stackoverflow.com/questions/8238473/cant-get-nsmousemoved-events-from-nexteventmatchingmask-with-an-nsopenglview - ProcessSerialNumber psn; - GetCurrentProcess(&psn); +/* ProcessSerialNumber psn; + GetCurrentProcess(&psn); TransformProcessType(&psn, kProcessTransformToForegroundApplication); SetFrontProcess(&psn); - + */ + m_retinaScaleFactor = [m_internalData->m_myview convertSizeToBacking:sz].width; [m_internalData->m_myApp finishLaunching]; @@ -808,45 +809,8 @@ void MacOpenGLWindow::startRendering() } } - if ([event type]== NSLeftMouseUp) - { - // printf("right mouse!"); - - NSPoint eventLocation = [event locationInWindow]; - NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil]; - m_mouseX = center.x; - m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y; - - - // printf("mouse coord = %f, %f\n",mouseX,mouseY); - if (m_mouseButtonCallback) - { - (*m_mouseButtonCallback)(0,0,m_mouseX,m_mouseY); - //handledEvent = true; - } - - } - - if ([event type]== NSLeftMouseDown) - { - // printf("right mouse!"); - - NSPoint eventLocation = [event locationInWindow]; - NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil]; - m_mouseX = center.x; - m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y; - - // printf("mouse coord = %f, %f\n",mouseX,mouseY); - if (m_mouseButtonCallback) - { - //handledEvent = true; - (*m_mouseButtonCallback)(0,1,m_mouseX,m_mouseY); - } - } - - - if ([event type]== NSRightMouseDown) + if (([event type]== NSRightMouseDown) || ([ event type]==NSLeftMouseDown)||([event type]==NSOtherMouseDown)) { // printf("right mouse!"); // float mouseX,mouseY; @@ -855,17 +819,39 @@ void MacOpenGLWindow::startRendering() NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil]; m_mouseX = center.x; m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y; - + int button=0; + switch ([event type]) + { + case NSLeftMouseDown: + { + button=0; + break; + } + case NSOtherMouseDown: + { + button=1; + break; + } + case NSRightMouseDown: + { + button=2; + break; + } + default: + { + + } + }; // printf("mouse coord = %f, %f\n",mouseX,mouseY); if (m_mouseButtonCallback) { //handledEvent = true; - (*m_mouseButtonCallback)(2,1,m_mouseX,m_mouseY); + (*m_mouseButtonCallback)(button,1,m_mouseX,m_mouseY); } } - if ([event type]== NSRightMouseUp) + if (([event type]== NSRightMouseUp) || ([ event type]==NSLeftMouseUp)||([event type]==NSOtherMouseUp)) { // printf("right mouse!"); // float mouseX,mouseY; @@ -875,9 +861,33 @@ void MacOpenGLWindow::startRendering() m_mouseX = center.x; m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y; + int button=0; + switch ([event type]) + { + case NSLeftMouseUp: + { + button=0; + break; + } + case NSOtherMouseUp: + { + button=1; + break; + } + case NSRightMouseUp: + { + button=2; + break; + } + default: + { + + } + }; + // printf("mouse coord = %f, %f\n",mouseX,mouseY); if (m_mouseButtonCallback) - (*m_mouseButtonCallback)(2,0,m_mouseX,m_mouseY); + (*m_mouseButtonCallback)(button,0,m_mouseX,m_mouseY); } @@ -899,7 +909,7 @@ void MacOpenGLWindow::startRendering() } } - if ([event type] == NSLeftMouseDragged) + if (([event type] == NSLeftMouseDragged) || ([event type] == NSRightMouseDragged) || ([event type] == NSOtherMouseDragged)) { int dx1, dy1; CGGetLastMouseDelta (&dx1, &dy1); diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp index 553a9da1a..e2ee9ab20 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp @@ -26,6 +26,7 @@ #include "OpenGLWindow/opengl_fontstashcallbacks.h" #include #include "OpenGLWindow/GLRenderToTexture.h" +#include "Bullet3Common/b3Quaternion.h" #ifdef _WIN32 #define popen _popen @@ -35,6 +36,7 @@ struct SimpleInternalData { GLuint m_fontTextureId; + GLuint m_largeFontTextureId; struct sth_stash* m_fontStash; OpenGL2RenderCallbacks* m_renderCallbacks; int m_droidRegular; @@ -48,8 +50,10 @@ struct SimpleInternalData static SimpleOpenGL3App* gApp=0; -static void SimpleResizeCallback( float width, float height) +static void SimpleResizeCallback( float widthf, float heightf) { + int width = (int)widthf; + int height = (int)heightf; gApp->m_instancingRenderer->resize(width,height); gApp->m_primRenderer->setScreenSize(width,height); @@ -111,7 +115,7 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height) m_window->setWindowTitle(title); - b3Assert(glGetError() ==GL_NO_ERROR); + b3Assert(glGetError() ==GL_NO_ERROR); glClearColor(0.9,0.9,1,1); m_window->startRendering(); @@ -156,6 +160,8 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height) TwGenerateDefaultFonts(); m_data->m_fontTextureId = BindFont(g_DefaultNormalFont); + m_data->m_largeFontTextureId = BindFont(g_DefaultLargeFont); + { @@ -189,12 +195,193 @@ struct sth_stash* SimpleOpenGL3App::getFontStash() return m_data->m_fontStash; } - - -void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY) +void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) { + float viewMat[16]; + float projMat[16]; + m_instancingRenderer->getCameraViewMatrix(viewMat); + m_instancingRenderer->getCameraProjectionMatrix(projMat); + + float camPos[4]; + this->m_instancingRenderer->getCameraPosition(camPos); + b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]); + b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ); + float dist = (cp-p).length(); + float dv = 0;//dist/1000.f; + // + //printf("str = %s\n",unicodeText); + + float dx=0; + + //int measureOnly=0; + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + int viewport[4]={0,0,m_instancingRenderer->getScreenWidth(),m_instancingRenderer->getScreenHeight()}; + + float posX = 450.f; + float posY = 100.f; + float winx,winy, winz; + + if (!projectWorldCoordToScreen(worldPosX, worldPosY, worldPosZ,viewMat,projMat,viewport,&winx, &winy, &winz)) + { + return; + } + posX = winx; + posY = m_instancingRenderer->getScreenHeight()/2+(m_instancingRenderer->getScreenHeight()/2)-winy; + + + if (0)//m_useTrueTypeFont) + { + bool measureOnly = false; + + float fontSize= 32;//64;//512;//128; + sth_draw_text(m_data->m_fontStash, + m_data->m_droidRegular,fontSize,posX,posY, + txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale()); + sth_end_draw(m_data->m_fontStash); + sth_flush_draw(m_data->m_fontStash); + } else + { + //float width = 0.f; + int pos=0; + //float color[]={0.2f,0.2,0.2f,1.f}; + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId); + + //float width = r.x; + //float extraSpacing = 0.; + + float startX = posX; + float startY = posY-g_DefaultLargeFont->m_CharHeight; + + + while (txt[pos]) + { + int c = txt[pos]; + //r.h = g_DefaultNormalFont->m_CharHeight; + //r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing; + float endX = startX+g_DefaultLargeFont->m_CharWidth[c]; + float endY = posY; + + + float currentColor[]={1.f,0.2,0.2f,1.f}; + + // m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultLargeFont->m_CharU0[c],g_DefaultLargeFont->m_CharV0[c],g_DefaultLargeFont->m_CharU1[c],g_DefaultLargeFont->m_CharV1[c]); + float u0 = g_DefaultLargeFont->m_CharU0[c]; + float u1 = g_DefaultLargeFont->m_CharU1[c]; + float v0 = g_DefaultLargeFont->m_CharV0[c]; + float v1 = g_DefaultLargeFont->m_CharV1[c]; + float color[4] = {currentColor[0],currentColor[1],currentColor[2],currentColor[3]}; + float x0 = startX; + float x1 = endX; + float y0 = startY; + float y1 = endY; + int screenWidth = m_instancingRenderer->getScreenWidth(); + int screenHeight = m_instancingRenderer->getScreenHeight(); + + float z = 2.f*winz-1.f;//*(far + float identity[16]={1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1}; + PrimVertex vertexData[4] = { + { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, + { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, + { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, + { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + }; + + m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,false); + + //DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]); + // DrawFilledRect(r); + + startX = endX; + //startY = endY; + + pos++; + + } + glBindTexture(GL_TEXTURE_2D,0); + } + + glDisable(GL_BLEND); + + +#if 0 + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + + int pos=0; + //float color[]={0.2f,0.2,0.2f,1.f}; + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId); + + //float width = r.x; + //float extraSpacing = 0.; + + float startX = posX; + float startY = posY; + + + while (txt[pos]) + { + float scaling = 0.02; + + int c = txt[pos]; + //r.h = g_DefaultNormalFont->m_CharHeight; + //r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing; + float endX = startX-float(g_DefaultLargeFont->m_CharWidth[c])*scaling; + float endY = startY-float(g_DefaultLargeFont->m_CharHeight)*scaling; + + + float currentColor[]={0.2f,0.2,0.2f,1.f}; + + float u0 = g_DefaultLargeFont->m_CharU0[c]; + float v0 = g_DefaultLargeFont->m_CharV0[c]; + float u1 = g_DefaultLargeFont->m_CharU1[c]; + float v1 = g_DefaultLargeFont->m_CharV1[c]; + float color[4] = {0,0,0,1}; + + PrimVertex vertexData[4] = { + { PrimVec4(startX, startY, 0, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, + { PrimVec4(startX, endY, 0 , 1.f), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, + { PrimVec4(endX, endY,0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, + { PrimVec4(endX,startY, 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + }; + float viewMat[16]; + float projMat[16]; + m_instancingRenderer->getCameraViewMatrix(viewMat); + m_instancingRenderer->getCameraProjectionMatrix(projMat); + + m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],viewMat,projMat,false); + + //DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]); + // DrawFilledRect(r); + + startX = endX; + //startY = endY; + + pos++; + + } + glBindTexture(GL_TEXTURE_2D,0); + + glDisable(GL_BLEND); +#endif + +} + + +void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi) +{ + + float posX = (float)posXi; + float posY = (float) posYi; // @@ -213,9 +400,15 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY) bool measureOnly = false; float fontSize= 64;//512;//128; + int bla=0; + float bla2=1; sth_draw_text(m_data->m_fontStash, m_data->m_droidRegular,fontSize,posX,posY, - txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale()); + txt,&dx, this->m_instancingRenderer->getScreenWidth(), + this->m_instancingRenderer->getScreenHeight(), + measureOnly, + m_window->getRetinaScale()); + sth_end_draw(m_data->m_fontStash); sth_flush_draw(m_data->m_fontStash); } else @@ -224,13 +417,13 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY) int pos=0; //float color[]={0.2f,0.2,0.2f,1.f}; glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D,m_data->m_fontTextureId); + glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId); //float width = r.x; //float extraSpacing = 0.; - int startX = posX; - int startY = posY; + float startX = posX; + float startY = posY; while (txt[pos]) @@ -238,13 +431,13 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY) int c = txt[pos]; //r.h = g_DefaultNormalFont->m_CharHeight; //r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing; - int endX = startX+g_DefaultNormalFont->m_CharWidth[c]; - int endY = startY+g_DefaultNormalFont->m_CharHeight; + float endX = startX+g_DefaultLargeFont->m_CharWidth[c]; + float endY = startY+g_DefaultLargeFont->m_CharHeight; float currentColor[]={0.2f,0.2,0.2f,1.f}; - m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]); + m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultLargeFont->m_CharU0[c],g_DefaultLargeFont->m_CharV0[c],g_DefaultLargeFont->m_CharU1[c],g_DefaultLargeFont->m_CharV1[c]); //DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]); // DrawFilledRect(r); @@ -290,11 +483,45 @@ int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f verts[i].u = cube_vertices[i*9+7]; verts[i].v = cube_vertices[i*9+8]; } - + int shapeId = m_instancingRenderer->registerShape(&verts[0].x,numVertices,cube_indices,numIndices); return shapeId; } +void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) +{ + b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); + cubeExtents[m_data->m_upAxis] = 0; + int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); + + b3Quaternion orn(0,0,0,1); + b3Vector3 center=b3MakeVector3(0,0,0,1); + b3Vector3 scaling=b3MakeVector3(1,1,1,1); + + for ( int i = 0; i < cells_x; i++) + { + for (int j = 0; j < cells_z; j++) + { + float* color =0; + if ((i + j) % 2 == 0) + { + color = (float*)color0; + } else { + color = (float*)color1; + } + if (this->m_data->m_upAxis==1) + { + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); + } else + { + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); + } + m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); + } + } + +} + int SimpleOpenGL3App::registerGraphicsSphereShape(float radius, bool usePointSprites, int largeSphereThreshold, int mediumSphereThreshold) { @@ -459,10 +686,10 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char* { for (int i=0;iendRendering(); if (m_data->m_frameDumpPngFileName) { - writeTextureToFile(m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(), - m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName, + writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(), + (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName, m_data->m_ffmpegFile); //m_data->m_renderTexture->disable(); //if (m_data->m_ffmpegFile==0) @@ -522,8 +749,8 @@ void SimpleOpenGL3App::swapBuffer() // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) { - int width = m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); - int height = m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight(); + int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); + int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight(); char cmd[8192]; sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.h b/btgui/OpenGLWindow/SimpleOpenGL3App.h index 9c3033ed0..54bb0e0b1 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL3App.h +++ b/btgui/OpenGLWindow/SimpleOpenGL3App.h @@ -19,9 +19,9 @@ struct SimpleOpenGL3App : public CommonGraphicsApp SimpleOpenGL3App(const char* title, int width,int height); virtual ~SimpleOpenGL3App(); - int registerCubeShape(float halfExtentsX=1.f,float halfExtentsY=1.f, float halfExtentsZ = 1.f); - int registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10); - + virtual int registerCubeShape(float halfExtentsX=1.f,float halfExtentsY=1.f, float halfExtentsZ = 1.f); + virtual int registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10); + virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]); void dumpNextFrameToPng(const char* pngFilename); void dumpFramesToVideo(const char* mp4Filename); @@ -31,6 +31,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp virtual void swapBuffer(); virtual void drawText( const char* txt, int posX, int posY); + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size); struct sth_stash* getFontStash(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 454c9c380..93868ceeb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -153,6 +153,7 @@ public: m_maxAppliedImpulse = maxImp; } + virtual void debugDraw(class btIDebugDraw* drawer)=0; }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 951808fad..891bd3099 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -756,6 +756,11 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* m_multiBodyConstraints.remove(constraint); } +void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint) +{ + constraint->debugDraw(getDebugDrawer()); +} + void btMultiBodyDynamicsWorld::debugDrawWorld() { @@ -772,11 +777,17 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (drawConstraints) { - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies + BT_PROFILE("btMultiBody debugDrawWorld"); + btAlignedObjectArray world_to_local; btAlignedObjectArray local_origin; + for (int c=0;cgetCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index 1295757a5..315fc7a60 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -56,6 +56,7 @@ public: m_pivotInB = pivotInB; } + virtual void debugDraw(class btIDebugDraw* drawer); }; From 2a1ac61c4da1e1217f919f4bda21e34eb2b2069c Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 27 Jan 2015 13:09:39 -0800 Subject: [PATCH 6/6] fix cmake build --- btgui/OpenGLWindow/CMakeLists.txt | 1 + btgui/OpenGLWindow/GLInstancingRenderer.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/btgui/OpenGLWindow/CMakeLists.txt b/btgui/OpenGLWindow/CMakeLists.txt index 57c476d8c..5d4460818 100644 --- a/btgui/OpenGLWindow/CMakeLists.txt +++ b/btgui/OpenGLWindow/CMakeLists.txt @@ -1,6 +1,7 @@ INCLUDE_DIRECTORIES( .. + ../../src ) FILE(GLOB OpenGLWindow_HDRS "*.h" ) diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 3fbe9035b..ec2d21921 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -27,7 +27,7 @@ float MOUSE_MOVE_MULTIPLIER = 0.4f; #include "OpenGLInclude.h" #include "b3gWindowInterface.h" -#include "Bullet3Common/b3MinMax.h" +//#include "Bullet3Common/b3MinMax.h" #ifndef __APPLE__ #ifndef glVertexAttribDivisor