diff --git a/CMakeLists.txt b/CMakeLists.txt index eebc38e67..badac4d99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,7 @@ IF(MSVC) IF (NOT USE_MSVC_RUNTIME_LIBRARY_DLL) #We statically link to reduce dependancies - FOREACH(flag_var CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO) + FOREACH(flag_var CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO ) IF(${flag_var} MATCHES "/MD") STRING(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}") ENDIF(${flag_var} MATCHES "/MD") @@ -231,18 +231,30 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + IF(BUILD_BULLET3) - OPTION(BUILD_BULLET3_DEMOS "Set when you want to build the Bullet 3 demos" ON) + IF(APPLE) + MESSAGE("Mac OSX Version is ${_CURRENT_OSX_VERSION}") + IF(_CURRENT_OSX_VERSION VERSION_LESS 10.9) + MESSAGE("Mac OSX below 10.9 has no OpenGL 3 support so please disable the BUILD_OPENGL3_DEMOS option") + #unset(BUILD_OPENGL3_DEMOS CACHE) + OPTION(BUILD_OPENGL3_DEMOS "Set when you want to build the OpenGL3+ demos" OFF) + ELSE() + OPTION(BUILD_OPENGL3_DEMOS "Set when you want to build the OpenGL3+ demos" ON) + ENDIF() + ELSE() + OPTION(BUILD_OPENGL3_DEMOS "Set when you want to build Bullet 3 OpenGL3+ demos" ON) + ENDIF() ELSE(BUILD_BULLET3) - unset(BUILD_BULLET3_DEMOS CACHE) - OPTION(BUILD_BULLET3_DEMOS "Set when you want to build the Bullet 3 demos" OFF) + unset(BUILD_OPENGL3_DEMOS CACHE) + OPTION(BUILD_OPENGL3_DEMOS "Set when you want to build Bullet 3 OpenGL3+ demos" OFF) ENDIF(BUILD_BULLET3) -IF(BUILD_BULLET3_DEMOS) +IF(BUILD_OPENGL3_DEMOS) IF(EXISTS ${BULLET_PHYSICS_SOURCE_DIR}/Demos3 AND IS_DIRECTORY ${BULLET_PHYSICS_SOURCE_DIR}/Demos3) SUBDIRS(Demos3) SUBDIRS(btgui) ENDIF() -ENDIF(BUILD_BULLET3_DEMOS) +ENDIF(BUILD_OPENGL3_DEMOS) OPTION(BUILD_EXTRAS "Set when you want to build the extras" ON) IF(BUILD_EXTRAS) @@ -267,10 +279,10 @@ ENDIF() IF(INSTALL_LIBS) SET (LIB_SUFFIX "" CACHE STRING "Define suffix of directory name (32/64)" ) - SET (LIB_DESTINATION "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}" CACHE STRING "Library directory name") + SET (LIB_DESTINATION "lib${LIB_SUFFIX}" CACHE STRING "Library directory name") ## the following are directories where stuff will be installed to - SET(INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/include/bullet/" CACHE PATH "The subdirectory to the header prefix") - SET(PKGCONFIG_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}/pkgconfig/" CACHE STRING "Base directory for pkgconfig files") + SET(INCLUDE_INSTALL_DIR "include/bullet/" CACHE PATH "The subdirectory to the header prefix") + SET(PKGCONFIG_INSTALL_PREFIX "lib${LIB_SUFFIX}/pkgconfig/" CACHE STRING "Base directory for pkgconfig files") IF(NOT WIN32) CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/bullet.pc.cmake ${CMAKE_CURRENT_BINARY_DIR}/bullet.pc @ONLY) INSTALL( @@ -297,7 +309,7 @@ list (APPEND BULLET_LIBRARIES LinearMath) list (APPEND BULLET_LIBRARIES BulletCollision) list (APPEND BULLET_LIBRARIES BulletDynamics) list (APPEND BULLET_LIBRARIES BulletSoftBody) -set (BULLET_USE_FILE ${CMAKE_INSTALL_PREFIX}/${BULLET_CONFIG_CMAKE_PATH}/UseBullet.cmake) +set (BULLET_USE_FILE ${BULLET_CONFIG_CMAKE_PATH}/UseBullet.cmake) configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/BulletConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake @ONLY ESCAPE_QUOTES diff --git a/Demos/BasicDemo/CMakeLists.txt b/Demos/BasicDemo/CMakeLists.txt index 16bb9e404..cff25d285 100644 --- a/Demos/BasicDemo/CMakeLists.txt +++ b/Demos/BasicDemo/CMakeLists.txt @@ -68,4 +68,4 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES DEBUG_POSTFIX "_Debug") SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") -ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) \ No newline at end of file +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) diff --git a/Demos/CMakeLists.txt b/Demos/CMakeLists.txt index 5c53f021e..9106fe712 100644 --- a/Demos/CMakeLists.txt +++ b/Demos/CMakeLists.txt @@ -24,18 +24,12 @@ IF (USE_GLUT) SET(SharedDemoSubdirs OpenGL ) ENDIF(BUILD_CPU_DEMOS) - IF(BUILD_MULTITHREADING) - SUBDIRS( MultiThreadedDemo OpenCLClothDemo ) - ENDIF(BUILD_MULTITHREADING) SUBDIRS( ${SharedDemoSubdirs} Benchmarks ) - IF(BUILD_MULTITHREADING) - SUBDIRS( ThreadingDemo VectorAdd_OpenCL ) - ENDIF() ENDIF(GLUT_FOUND) ELSE (USE_GLUT) diff --git a/Demos/CommonMultiBodySetup.h b/Demos/CommonMultiBodySetup.h new file mode 100644 index 000000000..03553e3c2 --- /dev/null +++ b/Demos/CommonMultiBodySetup.h @@ -0,0 +1,298 @@ + +#ifndef COMMON_MULTI_BODY_SETUP_H +#define COMMON_MULTI_BODY_SETUP_H + +//todo: replace this 'btBulletDynamicsCommon.h' header with specific used header files +#include "btBulletDynamicsCommon.h" +#include "CommonPhysicsSetup.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" + +struct CommonMultiBodySetup : public CommonPhysicsSetup +{ + //keep the collision shapes, for deletion/cleanup + btAlignedObjectArray m_collisionShapes; + btBroadphaseInterface* m_broadphase; + btCollisionDispatcher* m_dispatcher; + btMultiBodyConstraintSolver* m_solver; + btDefaultCollisionConfiguration* m_collisionConfiguration; + btMultiBodyDynamicsWorld* m_dynamicsWorld; + + //data for picking objects + class btRigidBody* m_pickedBody; + class btTypedConstraint* m_pickedConstraint; + class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; + + btVector3 m_oldPickingPos; + btVector3 m_hitPos; + btScalar m_oldPickingDist; + bool m_prevCanSleep; + + CommonMultiBodySetup() + :m_broadphase(0), + m_dispatcher(0), + m_solver(0), + m_collisionConfiguration(0), + m_dynamicsWorld(0), + m_pickedBody(0), + m_pickedConstraint(0), + m_pickingMultiBodyPoint2Point(0), + m_prevCanSleep(false) + { + } + + virtual void createEmptyDynamicsWorld() + { + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_collisionConfiguration->setConvexConvexMultipointIterations(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + m_solver = new btMultiBodyConstraintSolver; + + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + } + + + virtual void stepSimulation(float deltaTime) + { + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(deltaTime); + } + } + + + virtual void exitPhysics() + { + removePickingConstraint(); + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + + if (m_dynamicsWorld) + { + + int i; + for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) + { + m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i)); + } + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + } + //delete collision shapes + for (int j = 0; jdebugDrawWorld(); + } + + } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (m_dynamicsWorld==0) + return false; + + btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld); + + m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } else + { + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); + if (multiCol && multiCol->m_multiBody) + { + + m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling=1; + p2p->setMaxAppliedImpulse(2*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_pickingMultiBodyPoint2Point =p2p; + } + } + + + + // pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); + // printf("hit !\n"); + //add p2p + } + return false; + } + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + + btVector3 dir = rayToWorld-rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + + btVector3 newPivotB = rayFromWorld + dir; + pickCon->setPivotB(newPivotB); + } + } + + if (m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + + + btVector3 dir = rayToWorld-rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + + btVector3 newPivotB = rayFromWorld + dir; + + m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + + return false; + } + virtual void removePickingConstraint() + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + delete m_pickedConstraint; + m_pickedConstraint = 0; + m_pickedBody = 0; + } + if (m_pickingMultiBodyPoint2Point) + { + m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); + delete m_pickingMultiBodyPoint2Point; + m_pickingMultiBodyPoint2Point = 0; + } + } + + + + btBoxShape* createBoxShape(const btVector3& halfExtents) + { + btBoxShape* box = new btBoxShape(halfExtents); + return box; + } + + btRigidBody* createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape, const btVector4& color = btVector4(1, 0, 0, 1)) + { + btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE)); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + shape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + +#define USE_MOTIONSTATE 1 +#ifdef USE_MOTIONSTATE + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + + btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia); + + btRigidBody* body = new btRigidBody(cInfo); + //body->setContactProcessingThreshold(m_defaultContactProcessingThreshold); + +#else + btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia); + body->setWorldTransform(startTransform); +#endif// + + body->setUserIndex(-1); + m_dynamicsWorld->addRigidBody(body); + return body; + } +}; + +#endif //COMMON_MULTI_BODY_SETUP_H + diff --git a/Demos/CommonPhysicsSetup.h b/Demos/CommonPhysicsSetup.h index d436678e3..994fb9271 100644 --- a/Demos/CommonPhysicsSetup.h +++ b/Demos/CommonPhysicsSetup.h @@ -3,6 +3,7 @@ #define COMMON_PHYSICS_SETUP_H class btRigidBody; +class btCollisionObject; class btBoxShape; class btTransform; class btCollisionShape; @@ -18,6 +19,10 @@ struct GraphicsPhysicsBridge virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) { } + + virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) + { + } virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) { } @@ -32,11 +37,11 @@ struct GraphicsPhysicsBridge { return 0; } - + virtual void setUpAxis(int axis) { } - + }; struct CommonPhysicsSetup diff --git a/Demos/ConstraintDemo/CMakeLists.txt b/Demos/ConstraintDemo/CMakeLists.txt index d43a3bfa4..6051a2455 100644 --- a/Demos/ConstraintDemo/CMakeLists.txt +++ b/Demos/ConstraintDemo/CMakeLists.txt @@ -49,4 +49,4 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) SET_TARGET_PROPERTIES(AppConstraintDemo PROPERTIES DEBUG_POSTFIX "_Debug") SET_TARGET_PROPERTIES(AppConstraintDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") SET_TARGET_PROPERTIES(AppConstraintDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") -ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) \ No newline at end of file +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 3eab89698..317166ed2 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -443,7 +443,7 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y) m_debugMode |= btIDebugDraw::DBG_ProfileTimings; break; - case '=': + case '\\': { int maxSerializeBufferSize = 1024*1024*5; btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize); diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 30e7196a1..a471db017 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -462,7 +462,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons { const int s=x>>4; const GLubyte b=180; - GLubyte c=b+((s+t&1)&1)*(255-b); + GLubyte c=b+((s+(t&1))&1)*(255-b); pi[0]=pi[1]=pi[2]=pi[3]=c;pi+=3; } } @@ -518,7 +518,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons glColor3f(color.x(),color.y(), color.z()); - bool useWireframeFallback = true; + //bool useWireframeFallback = true; if (!(debugMode & btIDebugDraw::DBG_DrawWireframe)) { @@ -535,7 +535,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons const btSphereShape* sphereShape = static_cast(shape); float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin drawSphere(radius,10,10); - useWireframeFallback = false; + //useWireframeFallback = false; break; } @@ -586,7 +586,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons glEnd(); #endif - useWireframeFallback = false; + //useWireframeFallback = false; break; } @@ -617,7 +617,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons glTranslatef(0.0, 0.0, -0.5*height); glutSolidCone(radius,height,10,10); - useWireframeFallback = false; + //useWireframeFallback = false; break; } diff --git a/Demos/OpenGL/GlutStuff.cpp b/Demos/OpenGL/GlutStuff.cpp index c40de30e5..397303b16 100644 --- a/Demos/OpenGL/GlutStuff.cpp +++ b/Demos/OpenGL/GlutStuff.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -17,6 +17,11 @@ subject to the following restrictions: #include "DemoApplication.h" +#if !defined(_WIN32) && !defined(__APPLE__) +//assume linux workaround +//#include +#endif + //glut is C code, this global gDemoApplication links glut to the C++ demo static DemoApplication* gDemoApplication = 0; @@ -74,9 +79,17 @@ static void glutDisplayCallback(void) //#include int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp) { - + gDemoApplication = demoApp; +#if !defined(_WIN32) && !defined(__APPLE__) +//Access pthreads as a workaround for a bug in Linux/Ubuntu +//See https://bugs.launchpad.net/ubuntu/+source/nvidia-graphics-drivers-319/+bug/1248642 + +// int i=pthread_getconcurrency(); + // printf("pthread_getconcurrency()=%d\n",i); +#endif + glutInit(&argc, argv); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_STENCIL); glutInitWindowPosition(width/2, height/2); @@ -112,7 +125,7 @@ CGLSetParameter(cgl_context, kCGLCPSwapInterval, &swap_interval); #endif - + glutMainLoop(); return 0; } diff --git a/Demos/SerializeDemo/SerializeSetup.cpp b/Demos/SerializeDemo/SerializeSetup.cpp index 7cbdb4cc4..32e748cd8 100644 --- a/Demos/SerializeDemo/SerializeSetup.cpp +++ b/Demos/SerializeDemo/SerializeSetup.cpp @@ -3,19 +3,59 @@ SerializeSetup::SerializeSetup() { - + } SerializeSetup::~SerializeSetup() { - + } - + void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld); - const char* filename = "testFile.bullet"; - importer->loadFile(filename); -} \ No newline at end of file + const char* someFileName="spider.bullet"; + + const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; + int numPrefixes = sizeof(prefix)/sizeof(const char*); + char relativeFileName[1024]; + FILE* f=0; + bool fileFound = false; + int result = 0; + + for (int i=0;!f && iloadFile(relativeFileName); + + //for now, guess the up axis from gravity + if (m_dynamicsWorld->getGravity()[1] == 0.f) + { + gfxBridge.setUpAxis(2); + } else + { + gfxBridge.setUpAxis(1); + } + + +} + +void SerializeSetup::stepSimulation(float deltaTime) +{ + CommonRigidBodySetup::stepSimulation(deltaTime); +} diff --git a/Demos/SerializeDemo/SerializeSetup.h b/Demos/SerializeDemo/SerializeSetup.h index bb86e3a3f..7275d5b77 100644 --- a/Demos/SerializeDemo/SerializeSetup.h +++ b/Demos/SerializeDemo/SerializeSetup.h @@ -7,8 +7,9 @@ class SerializeSetup : public CommonRigidBodySetup public: SerializeSetup(); virtual ~SerializeSetup(); - + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + virtual void stepSimulation(float deltaTime); }; #endif //SERIALIZE_SETUP_H diff --git a/Demos/premake4.lua b/Demos/premake4.lua index 89ad341de..7b7b2ef35 100644 --- a/Demos/premake4.lua +++ b/Demos/premake4.lua @@ -34,7 +34,7 @@ function createDemos( demos, incdirs, linknames) linkoptions { "-framework Carbon -framework OpenGL -framework AGL -framework Glut" } configuration {"not Windows", "not MacOSX"} - links {"GL","GLU","glut"} + links {"GL","GLU","glut","pthread"} configuration{} diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index bf14d5bb5..a0162278c 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -14,10 +14,18 @@ #include "../bullet2/LuaDemo/LuaPhysicsSetup.h" #include "../bullet2/ChainDemo/ChainDemo.h" #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h" -#include "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h" +#include "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h" #include "../ImportURDFDemo/ImportURDFSetup.h" #include "../ImportObjDemo/ImportObjSetup.h" #include "../ImportSTLDemo/ImportSTLSetup.h" +#include "../../Demos/SerializeDemo/SerializeSetup.h" +#include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h" + +static BulletDemoInterface* TestJointTorqueCreateFunc(SimpleOpenGL3App* app) +{ + CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup(); + return new BasicDemo(app, physicsSetup); +} static BulletDemoInterface* LuaDemoCreateFunc(SimpleOpenGL3App* app) { @@ -36,7 +44,11 @@ static BulletDemoInterface* MyKinematicObjectCreateFunc(SimpleOpenGL3App* app) CommonPhysicsSetup* physicsSetup = new KinematicObjectSetup(); return new BasicDemo(app, physicsSetup); } - +static BulletDemoInterface* MySerializeCreateFunc(SimpleOpenGL3App* app) +{ + CommonPhysicsSetup* physicsSetup = new SerializeSetup(); + return new BasicDemo(app, physicsSetup); +} static BulletDemoInterface* MyConstraintCreateFunc(SimpleOpenGL3App* app) { CommonPhysicsSetup* physicsSetup = new ConstraintPhysicsSetup(); @@ -70,7 +82,7 @@ struct BulletDemoEntry static BulletDemoEntry allDemos[]= { - + //{"emptydemo",EmptyBulletDemo::MyCreateFunc}, {0,"API Demos", 0}, @@ -82,10 +94,11 @@ static BulletDemoEntry allDemos[]= {0,"File Formats", 0}, //@todo(erwincoumans) { 1, "bullet", MyImportSTLCreateFunc}, + { 1, ".bullet",MySerializeCreateFunc}, { 1, "Wavefront Obj", MyImportObjCreateFunc}, { 1, "URDF", MyImportUrdfCreateFunc }, { 1, "STL", MyImportSTLCreateFunc}, - + /* {1,"ChainDemo",ChainDemo::MyCreateFunc}, // {0, "Stress tests", 0 }, @@ -95,13 +108,15 @@ static BulletDemoEntry allDemos[]= {1,"LemkeHingeDemo",HingeDemo::LemkeCreateFunc}, {1,"InertiaHingeDemo",HingeDemo::InertiaCreateFunc}, {1,"ABMHingeDemo",HingeDemo::FeatherstoneCreateFunc}, - + {1,"Ragdoll",RagDollDemo::MyCreateFunc}, */ { 0, "Multibody" ,0}, {1,"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, // {"MultiBody2",FeatherstoneDemo2::MyCreateFunc}, {1,"MultiDofDemo",MultiDofDemo::MyCreateFunc}, + {1,"TestJointTorque",TestJointTorqueCreateFunc}, + }; @@ -124,10 +139,15 @@ static int loadCurrentDemoEntry(const char* startFileName) FILE* f = fopen(startFileName,"r"); if (f) { - fscanf(f,"%d",¤tEntry); + int result; + result = fscanf(f,"%d",¤tEntry); + if (result) + { + return currentEntry; + } fclose(f); } - return currentEntry; + return 0; }; #endif//BULLET_DEMO_ENTRIES_H diff --git a/Demos3/AllBullet2Demos/CMakeLists.txt b/Demos3/AllBullet2Demos/CMakeLists.txt index df3a53a84..535c1b9e6 100644 --- a/Demos3/AllBullet2Demos/CMakeLists.txt +++ b/Demos3/AllBullet2Demos/CMakeLists.txt @@ -5,8 +5,7 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/btgui ${BULLET_PHYSICS_SOURCE_DIR}/btgui/lua-5.2.3/src ) - - + SET(App_AllBullet2Demos_SRCS main.cpp BulletDemoInterface.h @@ -26,8 +25,16 @@ SET(App_AllBullet2Demos_SRCS ../../Demos/BasicDemo/BasicDemoPhysicsSetup.h ../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.cpp ../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h - ../../Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp - ../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h + ../../Demos/SerializeDemo/SerializeSetup.cpp + ../../Extras/Serialize/BulletFileLoader/bChunk.cpp + ../../Extras/Serialize/BulletFileLoader/bDNA.cpp + ../../Extras/Serialize/BulletFileLoader/bFile.cpp + ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp + ../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp + ../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp + ../bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp + ../bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp + ../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h ../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp ../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h ../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp @@ -93,7 +100,13 @@ ELSE(WIN32) MESSAGE(${COCOA}) link_libraries(${COCOA}) ELSE(APPLE) - LINK_LIBRARIES( pthread GLEW) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + LINK_LIBRARIES( pthread dl) ENDIF(APPLE) ENDIF(WIN32) diff --git a/Demos3/AllBullet2Demos/GraphingTexture.cpp b/Demos3/AllBullet2Demos/GraphingTexture.cpp index d81072298..19eac1ff8 100644 --- a/Demos3/AllBullet2Demos/GraphingTexture.cpp +++ b/Demos3/AllBullet2Demos/GraphingTexture.cpp @@ -14,7 +14,7 @@ GraphingTexture::~GraphingTexture() destroy(); } -bool GraphingTexture::destroy() +void GraphingTexture::destroy() { //TODO(erwincoumans) release memory etc... m_width = 0; @@ -58,21 +58,18 @@ bool GraphingTexture::create(int texWidth, int texHeight) glGenTextures(1,(GLuint*)&m_textureId); uploadImageData(); + return true; } void GraphingTexture::uploadImageData() { glBindTexture(GL_TEXTURE_2D,m_textureId); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_width,m_height,0,GL_RGBA,GL_UNSIGNED_BYTE,&m_imageData[0]); glGenerateMipmap(GL_TEXTURE_2D); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } diff --git a/Demos3/AllBullet2Demos/GraphingTexture.h b/Demos3/AllBullet2Demos/GraphingTexture.h index 33d209e69..79c62dbed 100644 --- a/Demos3/AllBullet2Demos/GraphingTexture.h +++ b/Demos3/AllBullet2Demos/GraphingTexture.h @@ -14,7 +14,7 @@ struct GraphingTexture virtual ~GraphingTexture(); bool create(int texWidth, int texHeight); - bool destroy(); + void destroy(); void setPixel(int x, int y, unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha) { diff --git a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp index be4802174..93ce77fff 100644 --- a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp +++ b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp @@ -124,7 +124,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) pSlider->onValueChanged.Add( handler, &MySliderEventHandler::SliderMoved ); handler->SliderMoved(pSlider); - float v = pSlider->GetValue(); +// float v = pSlider->GetValue(); m_gwenInternalData->m_curYposition+=22; } diff --git a/Demos3/AllBullet2Demos/GwenProfileWindow.cpp b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp index 86cae4d2e..84d78de03 100644 --- a/Demos3/AllBullet2Demos/GwenProfileWindow.cpp +++ b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp @@ -1,3 +1,4 @@ +#include "GwenProfileWindow.h" #include "../GpuDemos/gwenUserInterface.h" #include "../GpuDemos/gwenInternalData.h" #include "LinearMath/btQuickprof.h" @@ -27,7 +28,7 @@ protected: void SliderMoved(Gwen::Controls::Base* pControl ) { - Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl; + // Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl; //this->m_app->scaleYoungModulus(pSlider->GetValue()); // printf("Slider Value: %.2f", pSlider->GetValue() ); } @@ -35,8 +36,8 @@ protected: void OnCheckChangedStiffnessWarping (Gwen::Controls::Base* pControl) { - Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl; - bool checked = labeled->IsChecked(); + // Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl; +// bool checked = labeled->IsChecked(); //m_app->m_stiffness_warp_on = checked; } public: @@ -148,13 +149,13 @@ public: void UpdateText(CProfileIterator* profileIterator, bool idle) { - static bool update=true; + // static bool update=true; m_ctrl->SetBounds(0,0,this->GetInnerBounds().w,this->GetInnerBounds().h); // if (!update) // return; - update=false; + // update=false; static int test = 1; @@ -170,18 +171,18 @@ public: { //recompute profiling data, and store profile strings - char blockTime[128]; + // char blockTime[128]; - double totalTime = 0; + // double totalTime = 0; - int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + // int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); profileIterator->First(); double parent_time = profileIterator->Is_Root() ? time_since_reset : profileIterator->Get_Current_Parent_Total_Time(); - Gwen::Controls::TreeNode* curParent = m_node; + // Gwen::Controls::TreeNode* curParent = m_node; double accumulated_time = dumpRecursive(profileIterator,m_node); diff --git a/Demos3/AllBullet2Demos/GwenProfileWindow.h b/Demos3/AllBullet2Demos/GwenProfileWindow.h index cb68386bf..16058e6f5 100644 --- a/Demos3/AllBullet2Demos/GwenProfileWindow.h +++ b/Demos3/AllBullet2Demos/GwenProfileWindow.h @@ -1,8 +1,7 @@ #ifndef GWEN_PROFILE_WINDOW_H #define GWEN_PROFILE_WINDOW_H -struct MyProfileWindow* setupProfileWindow(struct GwenInternalData* data); - +class MyProfileWindow* setupProfileWindow(struct GwenInternalData* data); void processProfileData(MyProfileWindow* window, bool idle); void profileWindowSetVisible(MyProfileWindow* window, bool visible); void destroyProfileWindow(MyProfileWindow* window); diff --git a/Demos3/AllBullet2Demos/GwenTextureWindow.cpp b/Demos3/AllBullet2Demos/GwenTextureWindow.cpp index 72701c579..8d927d8fc 100644 --- a/Demos3/AllBullet2Demos/GwenTextureWindow.cpp +++ b/Demos3/AllBullet2Demos/GwenTextureWindow.cpp @@ -78,7 +78,7 @@ public: } }; -struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input) +MyGraphWindow* setupTextureWindow(const MyGraphInput& input) { MyGraphWindow* graphWindow = new MyGraphWindow(input); MyMenuItems2* menuItems = new MyMenuItems2(graphWindow); diff --git a/Demos3/AllBullet2Demos/GwenTextureWindow.h b/Demos3/AllBullet2Demos/GwenTextureWindow.h index a1080d761..da06dc55b 100644 --- a/Demos3/AllBullet2Demos/GwenTextureWindow.h +++ b/Demos3/AllBullet2Demos/GwenTextureWindow.h @@ -25,7 +25,7 @@ struct MyGraphInput { } }; -struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input); +class MyGraphWindow* setupTextureWindow(const MyGraphInput& input); void destroyTextureWindow(MyGraphWindow* window); diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 0dad18905..97eae1afe 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -24,9 +24,11 @@ static b3AlignedObjectArray allNames; bool drawGUI=true; extern bool useShadowMap; static bool wireframe=false; -static bool pauseSimulation=false; +static bool pauseSimulation=false;//true; int midiBaseIndex = 176; +//#include +//unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM); #ifdef B3_USE_MIDI #include "../../btgui/MidiTest/RtMidi.h" @@ -179,6 +181,32 @@ static void MyMouseButtonCallback(int button, int state, float x, float y) #include +void openURDFDemo(const char* filename) +{ + + if (sCurrentDemo) + { + sCurrentDemo->exitPhysics(); + app->m_instancingRenderer->removeAllInstances(); + delete sCurrentDemo; + sCurrentDemo=0; + } + + app->m_parameterInterface->removeAllParameters(); + + ImportUrdfDemo* physicsSetup = new ImportUrdfDemo(); + physicsSetup->setFileName(filename); + + sCurrentDemo = new BasicDemo(app, physicsSetup); + + if (sCurrentDemo) + { + sCurrentDemo->initPhysics(); + } + + +} + void selectDemo(int demoIndex) { sCurrentDemoIndex = demoIndex; @@ -238,13 +266,13 @@ struct MyMenuItemHander :public Gwen::Event::Handler void onButtonA(Gwen::Controls::Base* pControl) { - const Gwen::String& name = pControl->GetName(); + //const Gwen::String& name = pControl->GetName(); Gwen::Controls::TreeNode* node = (Gwen::Controls::TreeNode*)pControl; - Gwen::Controls::Label* l = node->GetButton(); + // Gwen::Controls::Label* l = node->GetButton(); Gwen::UnicodeString la = node->GetButton()->GetText();// node->GetButton()->GetName();// GetText(); Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); + // const char* ha = laa.c_str(); //printf("selected %s\n", ha); //int dep = but->IsDepressed(); @@ -257,7 +285,7 @@ struct MyMenuItemHander :public Gwen::Event::Handler Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); + //const char* ha = laa.c_str(); selectDemo(sCurrentHightlighted); @@ -265,10 +293,10 @@ struct MyMenuItemHander :public Gwen::Event::Handler } void onButtonC(Gwen::Controls::Base* pControl) { - Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; - Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); - Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); +// Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; + // Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); + // Gwen::String laa = Gwen::Utility::UnicodeToString(la); + // const char* ha = laa.c_str(); // printf("onButtonC ! %s\n", ha); @@ -314,7 +342,8 @@ struct GL3TexLoader : public MyTextureLoader virtual void LoadTexture( Gwen::Texture* pTexture ) { - const char* n = pTexture->name.Get().c_str(); + Gwen::String namestr = pTexture->name.Get(); + const char* n = namestr.c_str(); GLint* texIdPtr = m_hashMap[n]; if (texIdPtr) { @@ -326,6 +355,18 @@ struct GL3TexLoader : public MyTextureLoader } }; +void fileOpenCallback() +{ + + char filename[1024]; + int len = app->m_window->fileOpenDialog(filename,1024); + if (len) + { + //todo(erwincoumans) check if it is actually URDF + //printf("file open:%s\n", filename); + openURDFDemo(filename); + } +} extern float shadowMapWorldSize; int main(int argc, char* argv[]) @@ -334,7 +375,7 @@ int main(int argc, char* argv[]) b3Clock clock; - float dt = 1./120.f; + //float dt = 1./120.f; int width = 1024; int height=768; @@ -348,8 +389,7 @@ int main(int argc, char* argv[]) app->m_window->setKeyboardCallback(MyKeyboardCallback); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); sth_stash* fontstash=app->getFontStash(); gui = new GwenUserInterface; @@ -376,7 +416,8 @@ int main(int argc, char* argv[]) gt->create(256,256); int texId = gt->getTextureId(); myTexLoader->m_hashMap.insert("graph1", texId); - MyGraphWindow* gw = setupTextureWindow(input); + //MyGraphWindow* gw = + setupTextureWindow(input); } if (1) { @@ -403,7 +444,8 @@ int main(int argc, char* argv[]) int texId = gt->getTextureId(); input.m_xPos = width-input.m_width; myTexLoader->m_hashMap.insert("graph2", texId); - MyGraphWindow* gw = setupTextureWindow(input); + //MyGraphWindow* gw = + setupTextureWindow(input); } //destroyTextureWindow(gw); @@ -414,8 +456,8 @@ int main(int argc, char* argv[]) int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry); - char nodeText[1024]; - int curDemo = 0; + //char nodeText[1024]; + //int curDemo = 0; int selectedDemo = loadCurrentDemoEntry(startFileName); Gwen::Controls::TreeNode* curNode = tree; MyMenuItemHander* handler2 = new MyMenuItemHander(-1); @@ -472,12 +514,12 @@ int main(int argc, char* argv[]) */ unsigned long int prevTimeInMicroseconds = clock.getTimeMicroseconds(); - + gui->registerFileOpenCallback(fileOpenCallback); + do { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); app->m_instancingRenderer->init(); DrawGridData dg; dg.upAxis = app->getUpAxis(); diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index dc128f6a9..de38b356c 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -44,14 +44,23 @@ "../bullet2/BasicDemo/Bullet2RigidBodyDemo.h", "../bullet2/LuaDemo/LuaPhysicsSetup.cpp", "../bullet2/LuaDemo/LuaPhysicsSetup.h", + "../bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp", + "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h", -- "../DifferentialGearDemo/DifferentialGearSetup.cpp", -- "../DifferentialGearDemo/DifferentialGearSetup.h", "../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp", "../../Demos/BasicDemo/BasicDemoPhysicsSetup.h", "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.cpp", "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h", - "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp", - "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h", + "../../Demos/SerializeDemo/SerializeSetup.cpp", + "../../Extras/Serialize/BulletFileLoader/bChunk.cpp", + "../../Extras/Serialize/BulletFileLoader/bDNA.cpp", + "../../Extras/Serialize/BulletFileLoader/bFile.cpp", + "../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp", + "../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp", + "../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp", + "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp", + "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h", "../ImportURDFDemo/ImportURDFSetup.cpp", "../ImportObjDemo/ImportObjSetup.cpp", "../ImportSTLDemo/ImportSTLSetup.cpp", @@ -106,5 +115,5 @@ if os.is("Linux") then initX11() end if os.is("MacOSX") then - links{"Cocoa.framework"} + links{"Cocoa.framework"} end diff --git a/Demos3/BasicDemoCustomOpenGL2/main.cpp b/Demos3/BasicDemoCustomOpenGL2/main.cpp index e23b1d250..90ddcd5e0 100644 --- a/Demos3/BasicDemoCustomOpenGL2/main.cpp +++ b/Demos3/BasicDemoCustomOpenGL2/main.cpp @@ -144,7 +144,7 @@ int main(int argc, char* argv[]) } if (majorGlVersion>=3 && wci.m_openglVersion>=3) { - float retinaScale = 1.f; + // float retinaScale = 1.f; #ifndef __APPLE__ #ifndef _WIN32 @@ -154,14 +154,15 @@ int main(int argc, char* argv[]) glewInit(); #endif - //we ned to call glGetError twice, because of some Ubuntu/Intel/OpenGL issue + //we need to call glGetError twice, because of some Ubuntu/Intel/OpenGL issue - GLuint err = glGetError(); - err = glGetError(); - btAssert(err==GL_NO_ERROR); + glGetError(); + glGetError(); + + btAssert(glGetError()==GL_NO_ERROR); - retinaScale = window->getRetinaScale(); + //retinaScale = window->getRetinaScale(); //primRenderer = new GLPrimitiveRenderer(sWidth,sHeight); //sth_stash* font = initFont(primRenderer ); diff --git a/Demos3/CMakeLists.txt b/Demos3/CMakeLists.txt index 8984fe27c..21d271b99 100644 --- a/Demos3/CMakeLists.txt +++ b/Demos3/CMakeLists.txt @@ -1,5 +1,5 @@ if (OPENGL_FOUND) -if (BUILD_BULLET3_DEMOS) +if (BUILD_OPENGL3_DEMOS) SUBDIRS( AllBullet2Demos GpuDemos SimpleOpenGL3 ) -endif(BUILD_BULLET3_DEMOS) +endif(BUILD_OPENGL3_DEMOS) endif(OPENGL_FOUND) diff --git a/Demos3/GpuDemos/CMakeLists.txt b/Demos3/GpuDemos/CMakeLists.txt index b6fadf82e..0cb6f2c0e 100644 --- a/Demos3/GpuDemos/CMakeLists.txt +++ b/Demos3/GpuDemos/CMakeLists.txt @@ -50,7 +50,16 @@ ELSE(WIN32) MESSAGE(${COCOA}) link_libraries(${COCOA}) ELSE(APPLE) - LINK_LIBRARIES( GLEW X11 pthread dl) + SET(App_Bullet3_OpenCL_Demos_SRCS ${App_Bullet3_OpenCL_Demos_SRCS} ${App_Bullet3_OpenCL_Demos_Common_SRCS} ../../btgui/OpenGLWindow/GlewWindows/glew.c) + + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + + LINK_LIBRARIES( X11 pthread dl) ENDIF(APPLE) ENDIF(WIN32) diff --git a/Demos3/GpuDemos/ParticleDemo.cpp b/Demos3/GpuDemos/ParticleDemo.cpp index e68cfca65..ca14a8752 100644 --- a/Demos3/GpuDemos/ParticleDemo.cpp +++ b/Demos3/GpuDemos/ParticleDemo.cpp @@ -5,7 +5,7 @@ #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" #define MSTRINGIFY(A) #A -static char* particleKernelsString = +static const char* particleKernelsString = #include "ParticleKernels.cl" #define INTEROPKERNEL_SRC_PATH "demo/gpudemo/ParticleKernels.cl" @@ -40,7 +40,7 @@ static char* particleKernelsString = //#define NUM_PARTICLES_Y 30 //#define NUM_PARTICLES_Z 30 - + B3_ATTRIBUTE_ALIGNED16(struct) b3SimParams { @@ -48,7 +48,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3SimParams b3Vector3 m_gravity; float m_worldMin[4]; float m_worldMax[4]; - + float m_particleRad; float m_globalDamping; float m_boundaryDamping; @@ -59,7 +59,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3SimParams float m_attraction; float m_dummy; - + b3SimParams() { m_gravity.setValue(0,-.3,0.f); @@ -83,7 +83,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3SimParams struct ParticleInternalData { - + cl_kernel m_updatePositionsKernel; cl_kernel m_updatePositionsKernel2; @@ -92,7 +92,7 @@ struct ParticleInternalData cl_kernel m_collideParticlesKernel; b3GpuSapBroadphase* m_broadphaseGPU; - + cl_mem m_clPositionBuffer; @@ -102,7 +102,7 @@ struct ParticleInternalData b3AlignedObjectArray m_simParamCPU; b3OpenCLArray* m_simParamGPU; - + ParticleInternalData() : @@ -144,7 +144,7 @@ void ParticleDemo::exitCL() clReleaseKernel(m_data->m_updateAabbsKernel); clReleaseKernel(m_data->m_collideParticlesKernel); } - + GpuDemo::exitCL(); } @@ -158,12 +158,12 @@ void ParticleDemo::setupScene(const ConstructionInfo& ci) { initCL(ci.preferredOpenCLDeviceIndex,ci.preferredOpenCLPlatformIndex); - + int numParticles = NUM_PARTICLES_X*NUM_PARTICLES_Y*NUM_PARTICLES_Z; - + int maxObjects = NUM_PARTICLES_X*NUM_PARTICLES_Y*NUM_PARTICLES_Z+1024; - + int maxPairsSmallProxy = 32; float radius = m_data->m_simParamCPU[0].m_particleRad; @@ -219,15 +219,15 @@ void ParticleDemo::setupScene(const ConstructionInfo& ci) float position[4] = {0,0,0,0}; float quaternion[4] = {0,0,0,1}; - + float scaling[4] = {radius,radius,radius,1}; int userIndex = 0; int totalParticles = NUM_PARTICLES_X*NUM_PARTICLES_Y*NUM_PARTICLES_Z; - + int curColor = 0; - b3Vector4 colors[4] = + b3Vector4 colors[4] = { b3MakeVector4(1,1,1,1), b3MakeVector4(1,1,0.3,1), @@ -241,9 +241,9 @@ void ParticleDemo::setupScene(const ConstructionInfo& ci) float angle = b3RandRange(-B3_PI, B3_PI); for (int ii=0;iim_simParamCPU[0].m_particleRad; position[0] = arg*b3Cos(arg + angle); position[1] = 3.0f + arg; @@ -255,8 +255,8 @@ void ParticleDemo::setupScene(const ConstructionInfo& ci) int id = m_instancingRenderer->registerGraphicsInstance(shapeId,position,quaternion,color,scaling); - - void* userPtr = (void*)userIndex; + + int collidableIndex = userIndex; b3Vector3 aabbMin,aabbMax; b3Vector3 particleRadius=b3MakeVector3(rad,rad,rad); @@ -268,7 +268,7 @@ void ParticleDemo::setupScene(const ConstructionInfo& ci) angle += b3RandRange(-(float)B3_PI, (float)B3_PI); } } - + m_data->m_broadphaseGPU->writeAabbsToGpu(); @@ -291,7 +291,7 @@ void ParticleDemo::exitPhysics() void ParticleDemo::renderScene() { - + if (m_instancingRenderer) { m_instancingRenderer->renderScene(); @@ -315,7 +315,7 @@ void ParticleDemo::clientMoveAndDisplay() assert(err==GL_NO_ERROR); glFinish(); - + #if 1 @@ -326,10 +326,10 @@ void ParticleDemo::clientMoveAndDisplay() bool useCpu = false; if (useCpu) { - + float* posBuffer = (float*)hostPtr; - + for (int i=0;im_clQueue); } - - + + if (0) { - b3BufferInfoCL bInfo[] = { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_velocitiesGPU->getBufferCL(), true ), b3BufferInfoCL( m_data->m_clPositionBuffer) }; - + b3LauncherCL launcher(m_clData->m_clQueue, m_data->m_updatePositionsKernel,"m_updatePositionsKernel" ); launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); @@ -370,18 +370,18 @@ void ParticleDemo::clientMoveAndDisplay() launcher.launch1D( numParticles); clFinish(m_clData->m_clQueue); - + } if (1) { - b3BufferInfoCL bInfo[] = { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_clPositionBuffer), b3BufferInfoCL( m_data->m_velocitiesGPU->getBufferCL() ), b3BufferInfoCL( m_data->m_simParamGPU->getBufferCL(),true) }; - + b3LauncherCL launcher(m_clData->m_clQueue, m_data->m_updatePositionsKernel2 ,"m_updatePositionsKernel2"); launcher.setConst( numParticles); @@ -391,21 +391,21 @@ void ParticleDemo::clientMoveAndDisplay() launcher.launch1D( numParticles); clFinish(m_clData->m_clQueue); - + } if (0) { - b3BufferInfoCL bInfo[] = { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_clPositionBuffer), b3BufferInfoCL( m_data->m_broadphaseGPU->getAabbBufferWS()), }; - + b3LauncherCL launcher(m_clData->m_clQueue, m_data->m_updateAabbsKernel,"m_updateAabbsKernel" ); launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); launcher.setConst( m_data->m_simParamCPU[0].m_particleRad); launcher.setConst( numParticles); - + launcher.launch1D( numParticles); clFinish(m_clData->m_clQueue); } @@ -422,12 +422,12 @@ void ParticleDemo::clientMoveAndDisplay() if (numPairsGPU) { - b3BufferInfoCL bInfo[] = { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_clPositionBuffer), b3BufferInfoCL( m_data->m_velocitiesGPU->getBufferCL() ), b3BufferInfoCL( m_data->m_broadphaseGPU->getOverlappingPairBuffer(),true), }; - + b3LauncherCL launcher(m_clData->m_clQueue, m_data->m_collideParticlesKernel,"m_collideParticlesKernel"); launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); launcher.setConst( numPairsGPU); @@ -450,15 +450,15 @@ void ParticleDemo::clientMoveAndDisplay() //clReleaseMemObject(clBuffer); clFinish(m_clData->m_clQueue); - + } } - + #endif glUnmapBuffer( GL_ARRAY_BUFFER); glFlush(); - - + + } diff --git a/Demos3/GpuDemos/gwenInternalData.h b/Demos3/GpuDemos/gwenInternalData.h index 97681b463..8285cf5b8 100644 --- a/Demos3/GpuDemos/gwenInternalData.h +++ b/Demos3/GpuDemos/gwenInternalData.h @@ -40,6 +40,7 @@ struct GwenInternalData Gwen::Controls::TabButton* m_explorerPage; Gwen::Controls::TreeControl* m_explorerTreeCtrl; Gwen::Controls::MenuItem* m_viewMenu; + class MyMenuItems* m_menuItems; int m_curYposition; diff --git a/Demos3/GpuDemos/gwenUserInterface.cpp b/Demos3/GpuDemos/gwenUserInterface.cpp index 4db1347aa..55a18168f 100644 --- a/Demos3/GpuDemos/gwenUserInterface.cpp +++ b/Demos3/GpuDemos/gwenUserInterface.cpp @@ -44,13 +44,23 @@ class MyMenuItems : public Gwen::Controls::Base { public: - MyMenuItems() :Gwen::Controls::Base(0) + b3FileOpenCallback m_fileOpenCallback; + + MyMenuItems() :Gwen::Controls::Base(0),m_fileOpenCallback(0) { } void myQuitApp( Gwen::Controls::Base* pControl ) { exit(0); } + void fileOpen( Gwen::Controls::Base* pControl ) + { + if (m_fileOpenCallback) + { + (*m_fileOpenCallback)(); + } + } + }; struct MyTestMenuBar : public Gwen::Controls::MenuStrip @@ -58,17 +68,20 @@ struct MyTestMenuBar : public Gwen::Controls::MenuStrip Gwen::Controls::MenuItem* m_fileMenu; Gwen::Controls::MenuItem* m_viewMenu; + MyMenuItems* m_menuItems; MyTestMenuBar(Gwen::Controls::Base* pParent) :Gwen::Controls::MenuStrip(pParent) { // Gwen::Controls::MenuStrip* menu = new Gwen::Controls::MenuStrip( pParent ); { - MyMenuItems* menuItems = new MyMenuItems; + m_menuItems = new MyMenuItems; m_fileMenu = AddItem( L"File" ); - m_fileMenu->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp); - m_viewMenu = AddItem( L"View" ); + + m_fileMenu->GetMenu()->AddItem(L"Open",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::fileOpen); + m_fileMenu->GetMenu()->AddItem(L"Quit",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp); + m_viewMenu = AddItem( L"View" ); } } @@ -120,7 +133,7 @@ struct MyButtonHander :public Gwen::Event::Handler void onButtonA( Gwen::Controls::Base* pControl ) { Gwen::Controls::Button* but = (Gwen::Controls::Button*) pControl; - int dep = but->IsDepressed(); +// int dep = but->IsDepressed(); int tog = but->GetToggleState(); if (m_data->m_toggleButtonCallback) (*m_data->m_toggleButtonCallback)(m_buttonId,tog); @@ -142,6 +155,11 @@ void GwenUserInterface::setStatusBarMessage(const char* message, bool isLeft) } } +void GwenUserInterface::registerFileOpenCallback(b3FileOpenCallback callback) +{ + m_data->m_menuItems->m_fileOpenCallback = callback; +} + void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float retinaScale) { m_data->m_curYposition = 20; @@ -157,6 +175,10 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float MyTestMenuBar* menubar = new MyTestMenuBar(m_data->pCanvas); m_data->m_viewMenu = menubar->m_viewMenu; + m_data->m_menuItems = menubar->m_menuItems; + + + Gwen::Controls::StatusBar* bar = new Gwen::Controls::StatusBar(m_data->pCanvas); m_data->m_rightStatusBar = new Gwen::Controls::Label( bar ); m_data->m_rightStatusBar->SetWidth(width/2); diff --git a/Demos3/GpuDemos/gwenUserInterface.h b/Demos3/GpuDemos/gwenUserInterface.h index 5c7e3ee59..2d6cc14d4 100644 --- a/Demos3/GpuDemos/gwenUserInterface.h +++ b/Demos3/GpuDemos/gwenUserInterface.h @@ -5,7 +5,7 @@ struct GwenInternalData; typedef void (*b3ComboBoxCallback) (int combobox, const char* item); typedef void (*b3ToggleButtonCallback)(int button, int state); - +typedef void (*b3FileOpenCallback)(); class GwenUserInterface @@ -40,6 +40,8 @@ class GwenUserInterface void setStatusBarMessage(const char* message, bool isLeft=true); + void registerFileOpenCallback(b3FileOpenCallback callback); + GwenInternalData* getInternalData() { return m_data; diff --git a/Demos3/GpuDemos/main_opengl3core.cpp b/Demos3/GpuDemos/main_opengl3core.cpp index fde95d530..a84d4d994 100644 --- a/Demos3/GpuDemos/main_opengl3core.cpp +++ b/Demos3/GpuDemos/main_opengl3core.cpp @@ -55,8 +55,8 @@ bool dump_timings = false; int maxFrameCount = 102; extern char OpenSansData[]; extern char* gPairBenchFileName; -extern float shadowMapWidth; -extern float shadowMapHeight; +extern int shadowMapWidth; +extern int shadowMapHeight; extern bool gDebugLauncherCL; extern bool gAllowCpuOpenCL; extern bool gUseLargeBatches; @@ -94,7 +94,8 @@ static int loadCurrentDemoEntry(const char* startFileName) FILE* f = fopen(startFileName,"r"); if (f) { - fscanf(f,"%d",¤tEntry); + int bytesScanned; + bytesScanned = fscanf(f,"%d",¤tEntry); fclose(f); } return currentEntry; @@ -130,20 +131,20 @@ int selectedDemo = 1; GpuDemo::CreateFunc* allDemos[]= { //ConcaveCompound2Scene::MyCreateFunc, - - - + + + //ConcaveSphereScene::MyCreateFunc, - - + + // ConcaveSphereScene::MyCreateFunc, - + ConcaveScene::MyCreateFunc, - - + + GpuBoxPlaneScene::MyCreateFunc, GpuConstraintsDemo::MyCreateFunc, //GpuConvexPlaneScene::MyCreateFunc, @@ -155,14 +156,14 @@ GpuDemo::CreateFunc* allDemos[]= GpuSphereScene::MyCreateFunc, - - + + ConcaveSphereScene::MyCreateFunc, - + ConcaveCompoundScene::MyCreateFunc, - + //GpuTetraScene::MyCreateFunc, @@ -170,7 +171,7 @@ GpuDemo::CreateFunc* allDemos[]= Bullet2FileDemo::MyCreateFunc, - + PairBench::MyCreateFunc, GpuRaytraceScene::MyCreateFunc, @@ -534,7 +535,7 @@ FILE* defaultOutput = stdout; void myprintf(const char* msg) { - fprintf(defaultOutput,msg); + fprintf(defaultOutput,"%s",msg); } @@ -560,7 +561,7 @@ void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName char* pixels = (char*)malloc(textureWidth*textureHeight*numComponents); err=glGetError(); assert(err==GL_NO_ERROR); - + for (int j=0;jsetWindowTitle("Bullet 3.x GPU Rigid Body http://bulletphysics.org"); printf("-----------------------------------------------------\n"); - + #ifndef __APPLE__ @@ -732,7 +733,7 @@ int main(int argc, char* argv[]) GLPrimitiveRenderer prim(g_OpenGLWidth,g_OpenGLHeight); - + stash = initFont(&prim); @@ -782,7 +783,7 @@ int main(int argc, char* argv[]) glClearColor(1,1,1,1); glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);//|GL_STENCIL_BUFFER_BIT); glEnable(GL_DEPTH_TEST); - + sth_begin_draw(stash); //sth_draw_text(stash, droidRegular,12.f, dx, dy-50, "How does this OpenGL True Type font look? ", &dx,width,height); int spacing = 0;//g_OpenGLHeight; @@ -820,13 +821,13 @@ int main(int argc, char* argv[]) for (int i=0;iendRendering(); @@ -852,7 +853,7 @@ int main(int argc, char* argv[]) // demo->myinit(); bool useGpu = false; - + //int maxObjectCapacity=128*1024; int maxObjectCapacity=1024*1024; maxObjectCapacity = b3Max(maxObjectCapacity,ci.arraySizeX*ci.arraySizeX*ci.arraySizeX+10); @@ -874,7 +875,7 @@ int main(int argc, char* argv[]) - + printf("-----------------------------------------------------\n"); @@ -883,14 +884,14 @@ int main(int argc, char* argv[]) if (benchmark) { - + char prefixFileName[1024]; char csvFileName[1024]; char detailsFileName[1024]; b3OpenCLDeviceInfo info; b3OpenCLUtils::getDeviceInfo(demo->getInternalData()->m_clDevice,&info); - + //todo: move this time stuff into the Platform/Window class #ifdef _WIN32 SYSTEMTIME time; @@ -906,11 +907,11 @@ int main(int argc, char* argv[]) } sprintf(prefixFileName,"%s_%s_%s_%d_%d_%d_date_%d-%d-%d_time_%d-%d-%d",info.m_deviceName,buf,demoNames[selectedDemo],ci.arraySizeX,ci.arraySizeY,ci.arraySizeZ,time.wDay,time.wMonth,time.wYear,time.wHour,time.wMinute,time.wSecond); - + #else timeval now; gettimeofday(&now,0); - + struct tm* ptm; ptm = localtime (&now.tv_sec); char buf[1024]; @@ -926,7 +927,7 @@ int main(int argc, char* argv[]) ptm->tm_hour, ptm->tm_min, ptm->tm_sec); - + #endif sprintf(csvFileName,"%s.csv",prefixFileName); @@ -944,7 +945,7 @@ int main(int argc, char* argv[]) // fprintf(f,"%s (%dx%dx%d=%d),\n", g_deviceName,ci.arraySizeX,ci.arraySizeY,ci.arraySizeZ,ci.arraySizeX*ci.arraySizeY*ci.arraySizeZ); } - + fprintf(defaultOutput,"Demo settings:\n"); fprintf(defaultOutput," SelectedDemo=%d, demoname = %s\n", selectedDemo, demo->getName()); fprintf(defaultOutput," x_dim=%d, y_dim=%d, z_dim=%d\n",ci.arraySizeX,ci.arraySizeY,ci.arraySizeZ); @@ -971,7 +972,7 @@ int main(int argc, char* argv[]) if (exportFrame || exportMovie) { - + if (!renderTexture) { renderTexture = new GLRenderToTexture(); @@ -993,10 +994,10 @@ int main(int argc, char* argv[]) renderTexture->init(g_OpenGLWidth,g_OpenGLHeight,renderTextureId, RENDERTEXTURE_COLOR); } - + bool result = renderTexture->enable(); - } - + } + err = glGetError(); assert(err==GL_NO_ERROR); @@ -1052,10 +1053,10 @@ int main(int argc, char* argv[]) } */ - + if (exportFrame || exportMovie) { - + char fileName[1024]; sprintf(fileName,"screenShot%d.png",frameIndex++); writeTextureToPng(g_OpenGLWidth,g_OpenGLHeight,fileName); @@ -1085,7 +1086,7 @@ int main(int argc, char* argv[]) B3_PROFILE("glFinish"); } - + if (dump_timings) { @@ -1147,7 +1148,7 @@ int main(int argc, char* argv[]) { - + delete gui; gui=0; diff --git a/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp b/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp index 3d55dc89d..7ba6d6161 100644 --- a/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp +++ b/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp @@ -27,7 +27,7 @@ extern bool enableExperimentalCpuConcaveCollision; #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h" #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h" -///work-in-progress +///work-in-progress ///This ReadBulletSample is kept as simple as possible without dependencies to the Bullet SDK. ///It can be used to load .bullet data for other physics SDKs ///For a more complete example how to load and convert Bullet data using the Bullet SDK check out @@ -66,23 +66,23 @@ void createScene( GLInstancingRenderer& renderer,b3GpuNarrowPhase& np, b3GpuRigi //const char* fileName="../../bin/1000 convex.bullet"; //const char* fileName="../../bin/1000 stack.bullet"; //const char* fileName="../../bin/3000 fall.bullet"; - + //const char* fileName="../../bin/testFile.bullet"; - - + + FILE* f = fopen(fileName,"rb"); if (f) { fclose(f); - + bool verboseDumpAllTypes = false; bParse::b3BulletFile* bulletFile2 = new bParse::b3BulletFile(fileName); bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0; - + if (ok) bulletFile2->parse(verboseDumpAllTypes); else @@ -99,7 +99,7 @@ void createScene( GLInstancingRenderer& renderer,b3GpuNarrowPhase& np, b3GpuRigi printf("Error parsing file %s.\n",fileName); exit(0); } - + if (verboseDumpAllTypes) { bulletFile2->dumpChunks(bulletFile2->getFileDNA()); @@ -107,7 +107,7 @@ void createScene( GLInstancingRenderer& renderer,b3GpuNarrowPhase& np, b3GpuRigi b3BulletDataExtractor extractor(renderer,np,rbWorld); - + extractor.convertAllObjects(bulletFile2); delete bulletFile2; return; @@ -163,7 +163,7 @@ CONCAVE_SHAPES_START_HERE, GIMPACT_SHAPE_PROXYTYPE, ///Multimaterial mesh MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, - + EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CUSTOM_CONCAVE_SHAPE_TYPE, @@ -177,7 +177,7 @@ CONCAVE_SHAPES_END_HERE, INVALID_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES - + }; b3BulletDataExtractor::b3BulletDataExtractor(GLInstancingRenderer& renderer, b3GpuNarrowPhase& np, b3GpuRigidBodyPipeline& rbWorld) @@ -214,7 +214,7 @@ void b3BulletDataExtractor::convertAllObjects(bParse::b3BulletFile* bulletFile2) for (i=0;im_rigidBodies.size();i++) { - + Bullet3SerializeBullet2::b3RigidBodyFloatData* colObjData = (Bullet3SerializeBullet2::b3RigidBodyFloatData*)bulletFile2->m_rigidBodies[i]; Bullet3SerializeBullet2::b3CollisionShapeData* shapeData = (Bullet3SerializeBullet2::b3CollisionShapeData*)colObjData->m_collisionObjectData.m_collisionShape; for (int j=0;jm_rigidBodies.size();j++) { Bullet3SerializeBullet2::b3RigidBodyFloatData* colObjData = (Bullet3SerializeBullet2::b3RigidBodyFloatData*)m_instanceGroups[i]->m_rigidBodies[j]; - + b3Matrix3x3 mat; mat.deSerializeFloat((const b3Matrix3x3FloatData&)colObjData->m_collisionObjectData.m_worldTransform.m_basis); b3Quaternion orn; @@ -259,23 +259,23 @@ void b3BulletDataExtractor::convertAllObjects(bParse::b3BulletFile* bulletFile2) } if (keepStaticObjects || colObjData->m_inverseMass!=0.f) { - + m_rbPipeline.registerPhysicsInstance(mass,pos,quaternion,m_instanceGroups[i]->m_collisionShapeIndex,0,true); m_renderer.registerGraphicsInstance(m_instanceGroups[i]->m_collisionShapeIndex,pos,quaternion,color,m_graphicsShapes[i]->m_scaling); } - - + + } } } for (i=0;im_collisionObjects.size();i++) { - + } - + m_rbPipeline.writeAllInstancesToGpu(); } @@ -303,7 +303,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co case CONVEX_HULL_SHAPE_PROXYTYPE: { Bullet3SerializeBullet2::b3ConvexInternalShapeData* bsd = (Bullet3SerializeBullet2::b3ConvexInternalShapeData*)shapeData; - + switch (shapeData->m_shapeType) { case BOX_SHAPE_PROXYTYPE: @@ -331,7 +331,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co b3Vector3 pt = b3MakeVector3(convexData->m_unscaledPointsFloatPtr[i].m_floats[0], convexData->m_unscaledPointsFloatPtr[i].m_floats[1], convexData->m_unscaledPointsFloatPtr[i].m_floats[2]);//convexData->m_unscaledPointsFloatPtr[i].m_floats[3]); - + tmpPoints.push_back(pt*localScaling); } } @@ -378,7 +378,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co } }; - + break; } case CYLINDER_SHAPE_PROXYTYPE: @@ -408,9 +408,9 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co } }; - - + + break; } case MULTI_SPHERE_SHAPE_PROXYTYPE: @@ -426,7 +426,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co shape = new b3MultiSphereShape(&tmpPos[0],&radii[0],numSpheres); break; } - + #endif default: @@ -453,13 +453,13 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co //dat. } - + ///trimesh->m_meshInterface.m_meshPartsPtr //trimesh->m_meshInterface.m_scaling } //trimesh->m_meshInterface //b3TriangleIndexVertexArray* meshInterface = createMeshInterface(trimesh->m_meshInterface); - + //scaling //b3Vector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); @@ -489,7 +489,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co { printf("error: couldn't create childShape for compoundShape\n"); } - + } shape = compoundShape; @@ -519,7 +519,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co { return 0; } -#endif +#endif default: { printf("unsupported shape type (%d)\n",shapeData->m_shapeType); @@ -527,7 +527,7 @@ int b3BulletDataExtractor::convertCollisionShape( Bullet3SerializeBullet2::b3Co } return shapeIndex; - + } int b3BulletDataExtractor::createBoxShape( const Bullet3SerializeBullet2::b3Vector3FloatData& halfDimensions, const Bullet3SerializeBullet2::b3Vector3FloatData& localScaling, float collisionMargin) @@ -544,7 +544,7 @@ int b3BulletDataExtractor::createBoxShape( const Bullet3SerializeBullet2::b3Vect { int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); - + GraphicsShape* gfxShape = new GraphicsShape; gfxShape->m_vertices = cube_vertices; gfxShape->m_numvertices = numVertices; @@ -569,7 +569,7 @@ int b3BulletDataExtractor::createSphereShape( float radius, const Bullet3Seriali int b3BulletDataExtractor::createPlaneShape( const Bullet3SerializeBullet2::b3Vector3FloatData& planeNormal, float planeConstant, const Bullet3SerializeBullet2::b3Vector3FloatData& localScaling) { - printf("createPlaneShape with normal %f,%f,%f and planeConstant\n",planeNormal.m_floats[0], planeNormal.m_floats[1],planeNormal.m_floats[2],planeConstant); + printf("createPlaneShape with normal %f,%f,%f and planeConstant %f\n",planeNormal.m_floats[0], planeNormal.m_floats[1],planeNormal.m_floats[2],planeConstant); return -1; } @@ -582,7 +582,7 @@ GraphicsShape* b3BulletDataExtractor::createGraphicsShapeFromConvexHull(const b3 b3ConvexUtility* utilPtr = new b3ConvexUtility(); bool merge = true; utilPtr->initializePolyhedralFeatures(tmpPoints,numPoints,merge); - + b3AlignedObjectArray* vertices = new b3AlignedObjectArray; { int numVertices = utilPtr->m_vertices.size(); @@ -594,7 +594,7 @@ GraphicsShape* b3BulletDataExtractor::createGraphicsShapeFromConvexHull(const b3 b3Vector3 normal=b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]); if (face.m_indices.size()>2) { - + GraphicsVertex vtx; const b3Vector3& orgVertex = utilPtr->m_vertices[face.m_indices[0]]; vtx.xyzw[0] = orgVertex[0];vtx.xyzw[1] = orgVertex[1];vtx.xyzw[2] = orgVertex[2];vtx.xyzw[3] = 0.f; @@ -602,7 +602,7 @@ GraphicsShape* b3BulletDataExtractor::createGraphicsShapeFromConvexHull(const b3 vtx.uv[0] = 0.5f;vtx.uv[1] = 0.5f; int newvtxindex0 = vertices->size(); vertices->push_back(vtx); - + for (int j=1;jpush_back(newvtxindex0); @@ -630,15 +630,15 @@ GraphicsShape* b3BulletDataExtractor::createGraphicsShapeFromConvexHull(const b3 } } } - - + + GraphicsShape* gfxShape = new GraphicsShape; gfxShape->m_vertices = &vertices->at(0).xyzw[0]; gfxShape->m_numvertices = vertices->size(); gfxShape->m_indices = &indicesPtr->at(0); gfxShape->m_numIndices = indicesPtr->size(); for (int i=0;i<4;i++) - gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices + gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices return gfxShape; } -} \ No newline at end of file +} diff --git a/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp b/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp index e7eccc8e8..daf4c151a 100644 --- a/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp +++ b/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp @@ -27,13 +27,13 @@ GLInstanceGraphicsShape* createGraphicsShapeFromWavefrontObj(std::vector& shapes) { - + b3AlignedObjectArray* vertices = new b3AlignedObjectArray; { // int numVertices = obj->vertexCount; // int numIndices = 0; b3AlignedObjectArray* indicesPtr = new b3AlignedObjectArray; - + for (int s=0;ssize(); - + indicesPtr->push_back(vtxBaseIndex); indicesPtr->push_back(vtxBaseIndex+1); indicesPtr->push_back(vtxBaseIndex+2); - + GLInstanceVertex vtx0; vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0]; vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f]*3+1]; @@ -101,15 +101,15 @@ GLInstanceGraphicsShape* createGraphicsShapeFromWavefrontObj(std::vectorm_vertices = vertices; gfxShape->m_numvertices = vertices->size(); gfxShape->m_indices = indicesPtr; gfxShape->m_numIndices = indicesPtr->size(); for (int i=0;i<4;i++) - gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices + gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices return gfxShape; } } @@ -117,15 +117,15 @@ GLInstanceGraphicsShape* createGraphicsShapeFromWavefrontObj(std::vector verts; for (int i=0;im_numvertices;i++) @@ -167,9 +167,9 @@ void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci, const char* fil shape->m_vertices->at(i).xyzw[2]); verts.push_back(vtx*scaling); } - + int colIndex = m_data->m_np->registerConcaveMesh(&verts,shape->m_indices,b3MakeVector3(1,1,1)); - + { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; @@ -180,10 +180,10 @@ void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci, const char* fil int shapeId = ci.m_instancingRenderer->registerShape(&shape->m_vertices->at(0).xyzw[0], shape->m_numvertices, &shape->m_indices->at(0), shape->m_numIndices); b3Quaternion orn(0,0,0,1); - + b3Vector4 color=b3MakeVector4(0.3,0.3,1,1.f);//0.5);//1.f - - + + { float mass = 0.f; b3Vector3 position=b3MakeVector3(0,0,0); @@ -200,8 +200,8 @@ void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci, const char* fil } } - - + + } @@ -218,11 +218,11 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) //char* fileName = "teddy.obj";//"plane.obj"; // char* fileName = "sponza_closed.obj";//"plane.obj"; //char* fileName = "leoTest1.obj"; - char* fileName = "samurai_monastry.obj"; + const char* fileName = "samurai_monastry.obj"; // char* fileName = "teddy2_VHACD_CHs.obj"; - + b3Vector3 shift1=b3MakeVector3(0,0,0);//0,230,80);//150,-100,-120); - + b3Vector4 scaling=b3MakeVector4(10,10,10,1); // createConcaveMesh(ci,"plane100.obj",shift1,scaling); @@ -233,8 +233,8 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) // b3Vector3 shift3(130,-150,-75);//0,230,80);//150,-100,-120); // createConcaveMesh(ci,"leoTest1.obj",shift3,scaling); - createConcaveMesh(ci,fileName,shift1,scaling); - + createConcaveMesh(ci,fileName,shift1,scaling); + } else { @@ -250,17 +250,17 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); b3Vector3 position=b3MakeVector3(0,-2,0); b3Quaternion orn(0,0,0,1); - + b3Vector4 color=b3MakeVector4(0,0,1,1); - + int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); - + } } createDynamicObjects(ci); - + m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0}; @@ -285,15 +285,15 @@ void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci) int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; - - + + int index=0; - + if (1) { int curColor = 0; - b3Vector4 colors[4] = + b3Vector4 colors[4] = { b3MakeVector4(1,1,1,1), b3MakeVector4(1,1,0.3,1), @@ -345,14 +345,14 @@ void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci) 23+j*CONCAVE_GAPY, -(ci.arraySizeZ/2)*CONCAVE_GAPZ+k*CONCAVE_GAPZ); b3Quaternion orn(0,0,0,1); - + b3Vector4 color = colors[curColor]; curColor++; curColor&=3; - + int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); - + index++; } } @@ -377,10 +377,10 @@ void ConcaveCompoundScene::setupScene(const ConstructionInfo& ci) void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) { - char* fileName = "teddy2_VHACD_CHs.obj"; + const char* fileName = "teddy2_VHACD_CHs.obj"; //char* fileName = "cube_offset.obj"; - + b3Vector3 shift=b3MakeVector3(0,0,0);//0,230,80);//150,-100,-120); b3Vector4 scaling=b3MakeVector4(1,1,1,1); const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"}; @@ -388,12 +388,12 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) char relativeFileName[1024]; { - + int numPrefixes = sizeof(prefix)/sizeof(char*); for (int i=0;i vertexArray; b3AlignedObjectArray indexArray; - + //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); @@ -436,8 +436,8 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) b3Assert(stride2 == strideInBytes); { - - + + b3AlignedObjectArray childShapes; int numChildShapes = shapes.size(); @@ -455,7 +455,7 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) //for now, only support polyhedral child shapes b3GpuChildShape child; - + b3Vector3 pos=b3MakeVector3(0,0,0); b3Quaternion orn(0,0,0,1); for (int v=0;v<4;v++) @@ -463,21 +463,21 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) child.m_childPosition[v] = pos[v]; child.m_childOrientation[v] = orn[v]; } - + b3Transform tr; tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); int baseIndex = vertexArray.size(); - + for (int f=0;fm_np->registerCompoundShape(&childShapes); - + } //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size()); - b3Vector4 colors[4] = + b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), b3MakeVector4(0,0,1,1), b3MakeVector4(0,1,1,1), }; - + int curColor = 0; for (int i=0;i<1;i++)//ci.arraySizeX;i++) { @@ -569,14 +569,14 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) // b3Quaternion orn(0,0,0,1); b3Quaternion orn(b3MakeVector3(0,0,1),1.8); - + b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scaling=b3MakeVector4(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); - + index++; } } @@ -588,14 +588,14 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) void ConcaveCompoundScene::createDynamicObjects(const ConstructionInfo& ci) { - + int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); b3AlignedObjectArray vertexArray; b3AlignedObjectArray indexArray; - + //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; @@ -610,14 +610,14 @@ void ConcaveCompoundScene::createDynamicObjects(const ConstructionInfo& ci) { int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - + b3Vector3 childPositions[3] = { b3MakeVector3(0,-2,0), b3MakeVector3(0,0,0), b3MakeVector3(0,0,2) }; - + b3AlignedObjectArray childShapes; int numChildShapes = 3; for (int i=0;im_np->registerCompoundShape(&childShapes); - + } //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size()); - b3Vector4 colors[4] = + b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), b3MakeVector4(0,0,1,1), b3MakeVector4(0,1,1,1), }; - + int curColor = 0; for (int i=0;iregisterGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); - + index++; } } @@ -714,7 +714,7 @@ void ConcaveSphereScene::setupScene(const ConstructionInfo& ci) void ConcaveSphereScene::createDynamicObjects(const ConstructionInfo& ci) { - b3Vector4 colors[4] = + b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), @@ -737,20 +737,20 @@ void ConcaveSphereScene::createDynamicObjects(const ConstructionInfo& ci) { float mass = 1.f; - + b3Vector3 position=b3MakeVector3(-(ci.arraySizeX/2)*8+i*8,50+j*8,-(ci.arraySizeZ/2)*8+k*8); - + //b3Vector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3); - + b3Quaternion orn(0,0,0,1); - + b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scaling=b3MakeVector4(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); - + index++; } } diff --git a/Demos3/ImportObjDemo/ImportObjSetup.cpp b/Demos3/ImportObjDemo/ImportObjSetup.cpp index 294788740..fb65a7b08 100644 --- a/Demos3/ImportObjDemo/ImportObjSetup.cpp +++ b/Demos3/ImportObjDemo/ImportObjSetup.cpp @@ -28,7 +28,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector // int numIndices = 0; b3AlignedObjectArray* indicesPtr = new b3AlignedObjectArray; - for (int s=0;sm_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices); - int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); + //int id = + m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); /* diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp index ae20f3e58..bcdb0be3a 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp +++ b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp @@ -4,6 +4,7 @@ #include "OpenGLWindow/GLInstanceGraphicsShape.h" #include "btBulletDynamicsCommon.h" #include "OpenGLWindow/SimpleOpenGL3App.h" +#include "LoadMeshFromSTL.h" ImportSTLDemo::ImportSTLDemo(SimpleOpenGL3App* app) :m_app(app) @@ -16,97 +17,6 @@ ImportSTLDemo::~ImportSTLDemo() } -struct MySTLTriangle -{ - float normal[3]; - float vertex0[3]; - float vertex1[3]; - float vertex2[3]; -}; - -GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) -{ - GLInstanceGraphicsShape* shape = 0; - - FILE* file = fopen(relativeFileName,"rb"); - if (file) - { - int size=0; - if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) - { - printf("Error: Cannot access file to determine size of %s\n", relativeFileName); - } else - { - if (size) - { - printf("Open STL file of %d bytes\n",size); - char* memoryBuffer = new char[size+1]; - int actualBytesRead = fread(memoryBuffer,1,size,file); - if (actualBytesRead!=size) - { - printf("Error reading from file %s",relativeFileName); - } else - { - int numTriangles = *(int*)&memoryBuffer[80]; - - if (numTriangles) - { - - shape = new GLInstanceGraphicsShape; -// b3AlignedObjectArray* m_vertices; -// int m_numvertices; -// b3AlignedObjectArray* m_indices; -// int m_numIndices; -// float m_scaling[4]; - shape->m_scaling[0] = 1; - shape->m_scaling[1] = 1; - shape->m_scaling[2] = 1; - shape->m_scaling[3] = 1; - int index = 0; - shape->m_indices = new b3AlignedObjectArray(); - shape->m_vertices = new b3AlignedObjectArray(); - for (int i=0;ivertex0[v]; - v1.xyzw[v] = tri->vertex1[v]; - v2.xyzw[v] = tri->vertex2[v]; - v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v]; - } - v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f; - - shape->m_vertices->push_back(v0); - shape->m_vertices->push_back(v1); - shape->m_vertices->push_back(v2); - - shape->m_indices->push_back(index++); - shape->m_indices->push_back(index++); - shape->m_indices->push_back(index++); - - } - } - } - - delete[] memoryBuffer; - } - } - fclose(file); - } - shape->m_numIndices = shape->m_indices->size(); - shape->m_numvertices = shape->m_vertices->size(); - return shape; -} void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) @@ -143,7 +53,7 @@ void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) btVector3 shift(0,0,0); btVector3 scaling(10,10,10); - int index=10; +// int index=10; { @@ -161,7 +71,8 @@ void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices); - int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); + // int id = + m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); /* diff --git a/Demos3/ImportSTLDemo/LoadMeshFromSTL.h b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h new file mode 100644 index 000000000..f0fe15954 --- /dev/null +++ b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h @@ -0,0 +1,101 @@ + +#ifndef LOAD_MESH_FROM_STL_H +#define LOAD_MESH_FROM_STL_H + +#include "OpenGLWindow/GLInstanceGraphicsShape.h" +#include //fopen +#include "Bullet3Common/b3AlignedObjectArray.h" + +struct MySTLTriangle +{ + float normal[3]; + float vertex0[3]; + float vertex1[3]; + float vertex2[3]; +}; + +static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) +{ + GLInstanceGraphicsShape* shape = 0; + + FILE* file = fopen(relativeFileName,"rb"); + if (file) + { + int size=0; + if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) + { + printf("Error: Cannot access file to determine size of %s\n", relativeFileName); + } else + { + if (size) + { + printf("Open STL file of %d bytes\n",size); + char* memoryBuffer = new char[size+1]; + int actualBytesRead = fread(memoryBuffer,1,size,file); + if (actualBytesRead!=size) + { + printf("Error reading from file %s",relativeFileName); + } else + { + int numTriangles = *(int*)&memoryBuffer[80]; + + if (numTriangles) + { + + shape = new GLInstanceGraphicsShape; +// b3AlignedObjectArray* m_vertices; +// int m_numvertices; +// b3AlignedObjectArray* m_indices; +// int m_numIndices; +// float m_scaling[4]; + shape->m_scaling[0] = 1; + shape->m_scaling[1] = 1; + shape->m_scaling[2] = 1; + shape->m_scaling[3] = 1; + int index = 0; + shape->m_indices = new b3AlignedObjectArray(); + shape->m_vertices = new b3AlignedObjectArray(); + for (int i=0;ivertex0[v]; + v1.xyzw[v] = tri->vertex1[v]; + v2.xyzw[v] = tri->vertex2[v]; + v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v]; + } + v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f; + + shape->m_vertices->push_back(v0); + shape->m_vertices->push_back(v1); + shape->m_vertices->push_back(v2); + + shape->m_indices->push_back(index++); + shape->m_indices->push_back(index++); + shape->m_indices->push_back(index++); + + } + } + } + + delete[] memoryBuffer; + } + } + fclose(file); + } + shape->m_numIndices = shape->m_indices->size(); + shape->m_numvertices = shape->m_vertices->size(); + return shape; +} + +#endif //LOAD_MESH_FROM_STL_H diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index b5d9218df..dc9931e4a 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -1,6 +1,9 @@ #include "ImportURDFSetup.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "Bullet3Common/b3FileUtils.h" +#include "../ImportSTLDemo/LoadMeshFromSTL.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); @@ -9,7 +12,7 @@ static bool enableConstraints = true;//false; ImportUrdfDemo::ImportUrdfDemo() { - + sprintf(m_fileName,"r2d2.urdf"); } ImportUrdfDemo::~ImportUrdfDemo() @@ -18,7 +21,10 @@ ImportUrdfDemo::~ImportUrdfDemo() } - +void ImportUrdfDemo::setFileName(const char* urdfFileName) +{ + memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1); +} #include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" @@ -59,6 +65,8 @@ void printTree(my_shared_ptr link,int level = 0) struct URDF_LinkInformation { const Link* m_thisLink; + int m_linkIndex; + int m_parentIndex; btTransform m_localInertialFrame; btTransform m_localVisualFrame; @@ -66,7 +74,7 @@ struct URDF_LinkInformation btRigidBody* m_bulletRigidBody; virtual ~URDF_LinkInformation() { -printf("~\n"); + printf("~\n"); } }; @@ -80,9 +88,385 @@ struct URDF2BulletMappings { btHashMap m_link2rigidbody; btHashMap m_joint2Constraint; + + 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; + }; -void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings) +btCollisionShape* convertVisualToCollisionShape(const Visual* visual, const char* pathPrefix) +{ + btCollisionShape* shape = 0; + + switch (visual->geometry->type) + { + case Geometry::CYLINDER: + { + printf("processing a cylinder\n"); + urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); + + btAlignedObjectArray vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); + vertices.push_back(vert); + vert[2] = -cyl->length/2.; + vertices.push_back(vert); + + } + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->initializePolyhedralFeatures(); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); + //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + cylZShape->setMargin(0.001); + + shape = cylZShape; + break; + } + case Geometry::BOX: + { + printf("processing a box\n"); + 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); + shape = boxShape; + shape ->setMargin(0.001); + break; + } + case Geometry::SPHERE: + { + printf("processing a sphere\n"); + urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); + btScalar radius = sphere->radius; + btSphereShape* sphereShape = new btSphereShape(radius); + shape = sphereShape; + shape ->setMargin(0.001); + break; + + break; + } + case Geometry::MESH: + { + if (visual->name.length()) + { + printf("visual->name=%s\n",visual->name.c_str()); + } + if (visual->geometry) + { + const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); + if (mesh->filename.length()) + { + const char* filename = mesh->filename.c_str(); + printf("mesh->filename=%s\n",filename); + char fullPath[1024]; + sprintf(fullPath,"%s%s",pathPrefix,filename); + FILE* f = fopen(fullPath,"rb"); + if (f) + { + fclose(f); + GLInstanceGraphicsShape* glmesh = LoadMeshFromSTL(fullPath); + if (glmesh && (glmesh->m_numvertices>0)) + { + printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); + //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); + //convex->setUserIndex(shapeId); + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0;im_numvertices;i++) + { + convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); + } + //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); + btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + //cylZShape->initializePolyhedralFeatures(); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); + //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + cylZShape->setMargin(0.001); + shape = cylZShape; + } else + { + printf("issue extracting mesh from STL file %s\n", fullPath); + } + + } else + { + printf("mesh geometry not found %s\n",fullPath); + } + + + } + } + + + break; + } + default: + { + printf("Error: unknown visual geometry type\n"); + } + } + return shape; +} + +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->visual_array.size();v++) + { + const Visual* visual = link->visual_array[v].get(); + + shape = convertVisualToCollisionShape(visual,pathPrefix); + + if (shape)//childShape) + { + gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape); + + btVector3 color(0,0,1); + 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(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 URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix) { btCollisionShape* shape = 0; @@ -112,11 +496,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w)); } - btTransform parent2joint; + + btTransform parent2joint; + parent2joint.setIdentity(); if ((*link).parent_joint) { - btTransform p2j; + 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; @@ -130,72 +516,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { - printf("converting link %s",link->name.c_str()); - for (int v=0;vvisual_array.size();v++) + printf("converting visuals of link %s",link->name.c_str()); + for (int v=0;v<(int)link->visual_array.size();v++) { const Visual* visual = link->visual_array[v].get(); - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - - btAlignedObjectArray vertices; - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); - vertices.push_back(vert); - vert[2] = -cyl->length/2.; - vertices.push_back(vert); - - } - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->initializePolyhedralFeatures(); - //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - cylZShape->setMargin(0.001); - - shape = cylZShape; - break; - } - case Geometry::BOX: - { - printf("processing a box\n"); - 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); - shape = boxShape; - break; - } - case Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius*0.8; - btSphereShape* sphereShape = new btSphereShape(radius); - shape = sphereShape; - break; - - break; - } - case Geometry::MESH: - { - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - - - + shape = convertVisualToCollisionShape(visual,pathPrefix); + if (shape) { gfxBridge.createCollisionShapeGraphicsObject(shape); @@ -225,7 +552,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy btRigidBody* body = new btRigidBody(rbci); - + world->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask); // body->setFriction(0); @@ -241,25 +568,16 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy //create a joint if necessary if ((*link).parent_joint) { + btAssert(pp); + btRigidBody* parentBody =pp->m_bulletRigidBody; const Joint* pj = (*link).parent_joint.get(); btTransform offsetInA,offsetInB; - btTransform p2j; p2j.setIdentity(); - btVector3 p2jPos; p2jPos.setValue(pj->parent_to_joint_origin_transform.position.x, - pj->parent_to_joint_origin_transform.position.y, - pj->parent_to_joint_origin_transform.position.z); - btQuaternion p2jOrn;p2jOrn.setValue(pj->parent_to_joint_origin_transform.rotation.x, - pj->parent_to_joint_origin_transform.rotation.y, - pj->parent_to_joint_origin_transform.rotation.z, - pj->parent_to_joint_origin_transform.rotation.w); - - p2j.setOrigin(p2jPos); - p2j.setRotation(p2jOrn); - + offsetInA.setIdentity(); - offsetInA = pp->m_localVisualFrame.inverse()*p2j; + offsetInA = pp->m_localVisualFrame.inverse()*parent2joint; offsetInB.setIdentity(); offsetInB = visual_frame.inverse(); @@ -331,7 +649,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { if (*child) { - URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings); + URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix); } else @@ -353,32 +671,42 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( - //btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawContactPoints - //+btIDebugDraw::DBG_DrawAabb + +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); - + btVector3 gravity(0,0,0); gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); - int argc=0; - char* filename="somefile.urdf"; + //int argc=0; + char relativeFileName[1024]; + + b3FileUtils fu; + printf("m_fileName=%s\n", m_fileName); + bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024); - std::string xml_string; - if (argc < 2){ - std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; + std::string xml_string; + char pathPrefix[1024]; + pathPrefix[0] = 0; + + if (!fileFound){ + std::cerr << "URDF file not found, using a dummy test URDF" << std::endl; xml_string = std::string(urdf_char); } else { + + int maxPathLen = 1024; + fu.extractPath(relativeFileName,pathPrefix,maxPathLen); - std::fstream xml_file(filename, std::fstream::in); + std::fstream xml_file(relativeFileName, std::fstream::in); while ( xml_file.good() ) { std::string line; @@ -407,10 +735,46 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) printTree(root_link); btTransform worldTrans; worldTrans.setIdentity(); + + int numJoints = (*robot).m_numJoints; + if (1) { URDF2BulletMappings mappings; - URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings); + URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix); + } + + //the btMultiBody support is work-in-progress :-) + if (0) + { + URDF2BulletMappings mappings; + + btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints); + + mb->setHasSelfCollision(false); + mb->finalizeMultiDof(); + m_dynamicsWorld->addMultiBody(mb); + //m_dynamicsWorld->integrateTransforms(0.f); + + } + + + printf("numJoints/DOFS = %d\n", numJoints); + + if (0) + { + btVector3 halfExtents(1,1,1); + btBoxShape* box = new btBoxShape(halfExtents); + box->initializePolyhedralFeatures(); + + gfxBridge.createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 origin(0,0,0); + origin[upAxis]=5; + start.setOrigin(origin); + btRigidBody* body = createRigidBody(1,start,box); + btVector3 color(0.5,0.5,0.5); + gfxBridge.createRigidBodyGraphicsObject(body,color); } { @@ -418,16 +782,27 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); - + gfxBridge.createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-1.5; + groundOrigin[upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); + //m_dynamicsWorld->removeRigidBody(body); + // m_dynamicsWorld->addRigidBody(body,2,1); btVector3 color(0.5,0.5,0.5); gfxBridge.createRigidBodyGraphicsObject(body,color); } - + } + +void ImportUrdfDemo::stepSimulation(float deltaTime) +{ + if (m_dynamicsWorld) + { + //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + } +} diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.h b/Demos3/ImportURDFDemo/ImportURDFSetup.h index 043c6f438..9151804e2 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.h +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.h @@ -2,15 +2,20 @@ #define IMPORT_URDF_SETUP_H -#include "../../Demos/CommonRigidBodySetup.h" +#include "../../Demos/CommonMultiBodySetup.h" -class ImportUrdfDemo : public CommonRigidBodySetup +class ImportUrdfDemo : public CommonMultiBodySetup { + char m_fileName[1024]; + public: ImportUrdfDemo(); virtual ~ImportUrdfDemo(); - + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + virtual void stepSimulation(float deltaTime); + + void setFileName(const char* urdfFileName); }; #endif //IMPORT_URDF_SETUP_H diff --git a/Demos3/SimpleOpenGL3/CMakeLists.txt b/Demos3/SimpleOpenGL3/CMakeLists.txt index 417cef45a..7931eee85 100644 --- a/Demos3/SimpleOpenGL3/CMakeLists.txt +++ b/Demos3/SimpleOpenGL3/CMakeLists.txt @@ -27,7 +27,15 @@ ELSE(WIN32) MESSAGE(${COCOA}) link_libraries(${COCOA}) ELSE(APPLE) - LINK_LIBRARIES( GLEW X11 pthread dl Xext) + + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + + LINK_LIBRARIES( X11 pthread dl Xext) ENDIF(APPLE) ENDIF(WIN32) diff --git a/Demos3/SimpleOpenGL3/main.cpp b/Demos3/SimpleOpenGL3/main.cpp index 530d2b9a5..c97f8057b 100644 --- a/Demos3/SimpleOpenGL3/main.cpp +++ b/Demos3/SimpleOpenGL3/main.cpp @@ -11,15 +11,13 @@ int main(int argc, char* argv[]) { b3CommandLineArgs myArgs(argc,argv); - float dt = 1./120.f; SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768); app->m_instancingRenderer->setCameraDistance(13); app->m_instancingRenderer->setCameraPitch(0); app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); myArgs.GetCmdLineArgument("mp4_file",gVideoFileName); if (gVideoFileName) @@ -40,8 +38,7 @@ int main(int argc, char* argv[]) app->dumpNextFrameToPng(fileName); } - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); app->m_instancingRenderer->init(); app->m_instancingRenderer->updateCamera(); diff --git a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp index 016737643..f54aebf34 100644 --- a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp +++ b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp @@ -21,6 +21,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge { } virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color) + { + createCollisionObjectGraphicsObject(body,color); + } + virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color) { btCollisionShape* shape = body->getCollisionShape(); btTransform startTransform = body->getWorldTransform(); @@ -32,6 +36,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge } virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) { + //already has a graphics object? + if (collisionShape->getUserIndex()>=0) + return; + //todo: support all collision shape types switch (collisionShape->getShapeType()) { @@ -45,7 +53,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { - + break; } default: @@ -149,7 +157,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge { return m_glApp->m_parameterInterface; } - + virtual void setUpAxis(int axis) { m_glApp->setUpAxis(axis); @@ -233,7 +241,7 @@ btVector3 Bullet2RigidBodyDemo::getRayTo(int x,int y) btVector3 rightOffset; btVector3 cameraUp=btVector3(0,0,0); cameraUp[m_glApp->getUpAxis()]=1; - + btVector3 vertical = cameraUp; btVector3 hor; @@ -291,7 +299,7 @@ bool Bullet2RigidBodyDemo::mouseButtonCallback(int button, int state, float x, f btVector3 rayFrom = camPos; btVector3 rayTo = getRayTo(x,y); - + m_physicsSetup->pickBody(rayFrom, rayTo); diff --git a/Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp similarity index 100% rename from Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp rename to Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp diff --git a/Demos/ConstraintDemo/ConstraintPhysicsSetup.h b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.h similarity index 84% rename from Demos/ConstraintDemo/ConstraintPhysicsSetup.h rename to Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.h index e85853cb8..59cc66b08 100644 --- a/Demos/ConstraintDemo/ConstraintPhysicsSetup.h +++ b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.h @@ -1,7 +1,7 @@ #ifndef CONSTAINT_PHYSICS_SETUP_H #define CONSTAINT_PHYSICS_SETUP_H -#include "../CommonRigidBodySetup.h" +#include "../../../Demos/CommonRigidBodySetup.h" struct ConstraintPhysicsSetup : public CommonRigidBodySetup { diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 75b6b9cf2..34db331d8 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -169,7 +169,7 @@ bool Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y) //keep it at the same picking distance btVector3 newRayTo = getRayTo(x,y); btVector3 rayFrom; - btVector3 oldPivotInB = pickCon->getPivotInB(); +// btVector3 oldPivotInB = pickCon->getPivotInB(); btVector3 newPivotB; m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); btVector3 dir = newRayTo-rayFrom; @@ -186,7 +186,7 @@ bool Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y) btVector3 newRayTo = getRayTo(x,y); btVector3 rayFrom; - btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); + // btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); btVector3 newPivotB; btVector3 camPos; m_glApp->m_instancingRenderer->getCameraPosition(camPos); @@ -438,7 +438,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn btVector4 halfExtents(7.5,0.45,4.5,1); { - float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; @@ -484,7 +484,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn { btVector3 posr = local_origin[i+1]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; + //float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; @@ -531,8 +531,8 @@ void FeatherstoneDemo1::createGround() { //create ground int cubeShapeId = m_glApp->registerCubeShape(); - float pos[]={0,0,0}; - float orn[]={0,0,0,1}; + //float pos[]={0,0,0}; + //float orn[]={0,0,0,1}; { @@ -575,7 +575,8 @@ void FeatherstoneDemo1::initPhysics() settings.m_isFixedBase = false; settings.m_basePosition.setValue(0,10,0); settings.m_numLinks = 10; - btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); + //btMultiBody* mb = + createFeatherstoneMultiBody(m_dynamicsWorld,settings); m_glApp->m_instancingRenderer->writeTransforms(); @@ -727,7 +728,7 @@ class RagDoll2 hull->buildHull(0.01); { - int strideInBytes = 9*sizeof(float); + // int strideInBytes = 9*sizeof(float); int numVertices = hull->numVertices(); int numIndices =hull->numIndices(); @@ -988,7 +989,7 @@ public: virtual ~RagDoll2 () { - int i; + //int i; /* // Remove all constraints for ( i = 0; i < JOINT_COUNT; ++i) @@ -1027,8 +1028,8 @@ void FeatherstoneDemo2::initPhysics() settings.m_usePrismatic = true; btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); */ - btVector3 offset(0,2,0); - RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp); +// btVector3 offset(0,2,0); + //RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp); m_glApp->m_instancingRenderer->writeTransforms(); diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp index f31130428..0f76467bd 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp @@ -307,7 +307,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod { - float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; @@ -348,7 +348,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod { btVector3 posr = local_origin[i+1]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; diff --git a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp index ad87a0a50..ba2669de9 100644 --- a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp +++ b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp @@ -14,7 +14,7 @@ extern "C" { } -char* sLuaFileName = "init_physics.lua"; +const char* sLuaFileName = "init_physics.lua"; static const float scaling=0.35f; static LuaPhysicsSetup* sLuaDemo = 0; diff --git a/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp new file mode 100644 index 000000000..f253d6f37 --- /dev/null +++ b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp @@ -0,0 +1,251 @@ +//test addJointTorque +#include "TestJointTorqueSetup.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +TestJointTorqueSetup::TestJointTorqueSetup() +{ +} + +TestJointTorqueSetup::~TestJointTorqueSetup() +{ + +} + +void TestJointTorqueSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + int upAxis = 2; + + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + + + + gfxBridge.setUpAxis(upAxis); + + this->createEmptyDynamicsWorld(); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + //btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + //create a static ground object + if (0) + { + btVector3 groundHalfExtents(20,20,20); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + gfxBridge.createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,0); + groundOrigin[upAxis]=-1.5; + start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + gfxBridge.createRigidBodyGraphicsObject(body,color); + } + + { + bool floating = false; + bool damping = true; + bool gyro = true; + int numLinks = 5; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if(baseMass) + { + btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool isMultiDof = false; + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + m_multiBody = pMultiBody; + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + // pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + if(!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + + //pMultiBody->finalizeMultiDof(); + + btMultiBodyDynamicsWorld* world = m_dynamicsWorld; + + /// + world->addMultiBody(pMultiBody); + btMultiBody* mbC = pMultiBody; + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + btVector3 gravity(0,0,0); + //gravity[upAxis] = -9.81; + m_dynamicsWorld->setGravity(gravity); + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 45.f * SIMD_PI/ 180.f; + if(!spherical) + if(mbC->isMultiDof()) + mbC->setJointPosMultiDof(0, &q0); + else + mbC->setJointPos(0, q0); + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + double friction = 1; + { + + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + gfxBridge.createCollisionShapeGraphicsObject(box); + + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); +//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); + + world->addCollisionObject(col, 2,1+2); + + btVector3 color(0.0,0.0,0.5); + gfxBridge.createCollisionObjectGraphicsObject(col,color); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + + btVector3 posr = local_origin[i+1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + gfxBridge.createCollisionShapeGraphicsObject(box); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + world->addCollisionObject(col,2,1+2); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + gfxBridge.createCollisionObjectGraphicsObject(col,color); + + + pMultiBody->getLink(i).m_collider=col; + } + } + +} + +void TestJointTorqueSetup::stepSimulation(float deltaTime) +{ + m_multiBody->addJointTorque(0, 10.0); + m_dynamicsWorld->stepSimulation(deltaTime); +} + diff --git a/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.h b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.h new file mode 100644 index 000000000..89ae864fe --- /dev/null +++ b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.h @@ -0,0 +1,21 @@ +#ifndef TEST_JOINT_TORQUE_SETUP_H +#define TEST_JOINT_TORQUE_SETUP_H + +#include "../../../Demos/CommonMultiBodySetup.h" + +struct TestJointTorqueSetup : public CommonMultiBodySetup +{ + btMultiBody* m_multiBody; + +public: + + TestJointTorqueSetup(); + virtual ~TestJointTorqueSetup(); + + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + + virtual void stepSimulation(float deltaTime); + +}; +#endif //TEST_JOINT_TORQUE_SETUP_H + diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index 982931e0c..5e6857faa 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -197,10 +197,46 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData* } break; } + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API + //so deal with this + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; + + switch (capData->m_upAxis) + { + case 0: + { + shape = createCapsuleShapeX(1,1); + break; + } + case 1: + { + shape = createCapsuleShapeY(1,1); + break; + } + case 2: + { + shape = createCapsuleShapeZ(1,1); + break; + } + default: + { + printf("error: wrong up axis for btCapsuleShape\n"); + } + + + }; + if (shape) + { + btCapsuleShape* cap = (btCapsuleShape*) shape; + cap->deSerializeFloat(capData); + } + break; + } case CYLINDER_SHAPE_PROXYTYPE: case CONE_SHAPE_PROXYTYPE: - case CAPSULE_SHAPE_PROXYTYPE: case BOX_SHAPE_PROXYTYPE: case SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE: @@ -227,36 +263,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData* shape = createSphereShape(implicitShapeDimensions.getX()); break; } - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; - switch (capData->m_upAxis) - { - case 0: - { - shape = createCapsuleShapeX(implicitShapeDimensions.getY()+bsd->m_collisionMargin*2,2*implicitShapeDimensions.getX()); - break; - } - case 1: - { - shape = createCapsuleShapeY(implicitShapeDimensions.getX()+bsd->m_collisionMargin*2,2*implicitShapeDimensions.getY()); - break; - } - case 2: - { - shape = createCapsuleShapeZ(implicitShapeDimensions.getX()+bsd->m_collisionMargin*2,2*implicitShapeDimensions.getZ()); - break; - } - default: - { - printf("error: wrong up axis for btCapsuleShape\n"); - } - bsd->m_collisionMargin = 0.f; - - }; - - break; - } + case CYLINDER_SHAPE_PROXYTYPE: { btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; diff --git a/readme.txt b/README.md similarity index 92% rename from readme.txt rename to README.md index 5dc5f6a6a..373cb3811 100644 --- a/readme.txt +++ b/README.md @@ -1,3 +1,5 @@ +[![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3) +[![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3) Bullet 3.x GPU rigid body pipeline using OpenCL. diff --git a/appveyor.yml b/appveyor.yml new file mode 100644 index 000000000..54c1175f7 --- /dev/null +++ b/appveyor.yml @@ -0,0 +1,9 @@ +build: + project: build3/vs2010/0_Bullet3Solution.sln + +before_build: + - echo %CD% + - ps: cd build3 + - echo %CD% + - premake4 vs2010 + - ps: cd .. diff --git a/btgui/Gwen/Controls/CrossSplitter.cpp b/btgui/Gwen/Controls/CrossSplitter.cpp index 2d52d61cc..f80c7255d 100644 --- a/btgui/Gwen/Controls/CrossSplitter.cpp +++ b/btgui/Gwen/Controls/CrossSplitter.cpp @@ -112,7 +112,7 @@ void CrossSplitter::Layout( Skin::Base* /*skin*/ ) else { //This should probably use Fill docking instead - m_Sections[m_iZoomedSection]->SetBounds( 0, 0, Width(), Height() ); + m_Sections[(int)m_iZoomedSection]->SetBounds( 0, 0, Width(), Height() ); } } diff --git a/btgui/Gwen/Controls/Resizer.cpp b/btgui/Gwen/Controls/Resizer.cpp index c920d740e..b3506f849 100644 --- a/btgui/Gwen/Controls/Resizer.cpp +++ b/btgui/Gwen/Controls/Resizer.cpp @@ -23,7 +23,7 @@ void Resizer::OnMouseMoved( int x, int y, int /*deltaX*/, int /*deltaY*/ ) if ( !m_pTarget ) return; if ( !m_bDepressed ) return; - Gwen::Rect oldBounds = m_pTarget->GetBounds(); +// Gwen::Rect oldBounds = m_pTarget->GetBounds(); Gwen::Rect pBounds = m_pTarget->GetBounds(); Gwen::Point pntMin = m_pTarget->GetMinimumSize(); diff --git a/btgui/Gwen/Controls/ScrollControl.cpp b/btgui/Gwen/Controls/ScrollControl.cpp index 023ade942..4a0a95231 100644 --- a/btgui/Gwen/Controls/ScrollControl.cpp +++ b/btgui/Gwen/Controls/ScrollControl.cpp @@ -134,8 +134,14 @@ void ScrollControl::UpdateScrollBars() m_InnerPanel->SetSize( Utility::Max(Width(), childrenWidth), Utility::Max(Height(), childrenHeight)); - float wPercent = (float)Width() / (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width())); - float hPercent = (float)Height() / (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height())); + float hg = (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width())); + if (hg==0.f) + hg = 0.00001f; + float wPercent = (float)Width() / hg; + hg = (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height())); + if (hg==0.f) + hg = 0.00001f; + float hPercent = (float)Height() / hg; if ( m_bCanScrollV ) SetVScrollRequired( hPercent >= 1 ); diff --git a/btgui/Gwen/Controls/TreeControl.cpp b/btgui/Gwen/Controls/TreeControl.cpp index 972ec8456..bb53413fe 100644 --- a/btgui/Gwen/Controls/TreeControl.cpp +++ b/btgui/Gwen/Controls/TreeControl.cpp @@ -101,12 +101,12 @@ bool TreeControl::OnKeyUp( bool bDown ) if (bDown) { ForceUpdateScrollBars(); - int maxIndex = 0; +// int maxIndex = 0; int newIndex = 0; int maxItem=0; int curItem=-1; iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem); - maxIndex = maxItem; + // maxIndex = maxItem; int targetItem = curItem; if (curItem>0) { @@ -121,29 +121,32 @@ bool TreeControl::OnKeyUp( bool bDown ) iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex); } curItem = newIndex; - float amount = float(newIndex)/float(maxIndex); + // float amount = float(newIndex)/float(maxIndex); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + //float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + // float maxCoordSelectedItem = (curItem+1)*16.f; + if (contSize!=viewSize) { - float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); - if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } - } - { - int numItems = (viewSize)/16-1; - float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); - - if (newAmount>curAmount) { - m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + int numItems = (viewSize)/16-1; + float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); + + if (newAmount>curAmount) + { + m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } } } @@ -157,12 +160,12 @@ bool TreeControl::OnKeyDown( bool bDown ) if (bDown) { ForceUpdateScrollBars(); - int maxIndex = 0; + // int maxIndex = 0; int newIndex = 0; int maxItem=0; int curItem=-1; iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem); - maxIndex = maxItem; + // maxIndex = maxItem; int targetItem = curItem; if (curItem>=0) { @@ -177,29 +180,32 @@ bool TreeControl::OnKeyDown( bool bDown ) iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex); } curItem= newIndex; - float amount = (int)float(newIndex)/float(maxIndex); + // float amount = (int)float(newIndex)/float(maxIndex); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + //float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + //float maxCoordSelectedItem = (curItem+1)*16.f; + if (contSize!=viewSize) { - float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); - if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } - } - { - int numItems = (viewSize)/16-1; - float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); - - if (newAmount>curAmount) { - m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + int numItems = (viewSize)/16-1; + float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); + + if (newAmount>curAmount) + { + m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } } } @@ -219,29 +225,32 @@ bool TreeControl::OnKeyRight( bool bDown ) int maxItem=0; int curItem=0; iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem); - float amount = float(curItem)/float(maxItem); + // float amount = float(curItem)/float(maxItem); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; +// float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + // float maxCoordSelectedItem = (curItem+1)*16.f; + if (contSize!=viewSize) { - float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); - if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } - } - { - int numItems = (viewSize)/16-1; - float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); - - if (newAmount>curAmount) { - m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + int numItems = (viewSize)/16-1; + float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); + + if (newAmount>curAmount) + { + m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } } Invalidate(); @@ -262,34 +271,36 @@ bool TreeControl::OnKeyLeft( bool bDown ) int maxItems=0; int curItem=0; iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItems,&curItem); - float amount = float(curItem)/float(maxItems); + // float amount = float(curItem)/float(maxItems); // m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(amount,true); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + // float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; - + // float maxCoordSelectedItem = (curItem+1)*16.f; + if (contSize!=viewSize) { - float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); - if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } } - } - { - int numItems = (viewSize)/16-1; - float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); + { + int numItems = (viewSize)/16-1; + float newAmount = float((curItem-numItems)*16)/(contSize-viewSize); - if (newAmount>curAmount) - { - m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + if (newAmount>curAmount) + { + m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true); + } + Invalidate(); } - Invalidate(); } //viewSize/contSize diff --git a/btgui/Gwen/Controls/TreeNode.cpp b/btgui/Gwen/Controls/TreeNode.cpp index ba694567c..df3945b8c 100644 --- a/btgui/Gwen/Controls/TreeNode.cpp +++ b/btgui/Gwen/Controls/TreeNode.cpp @@ -73,7 +73,7 @@ void TreeNode::Render( Skin::Base* skin ) TreeNode* TreeNode::AddNode( const UnicodeString& strLabel ) { - int sz = sizeof(TreeNode); +// int sz = sizeof(TreeNode); TreeNode* node = new TreeNode( this ); node->SetText( strLabel ); node->Dock( Pos::Top ); @@ -231,7 +231,7 @@ void TreeNode::iterate(int action, int* curIndex, int* targetIndex) Gwen::String name = Gwen::Utility::UnicodeToString(m_Title->GetText()); - int actualIndex = curIndex? *curIndex : -1; +// int actualIndex = curIndex? *curIndex : -1; //printf("iterated over item %d with name = %s\n", actualIndex, name.c_str()); if (action==ITERATE_ACTION_SELECT) diff --git a/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp b/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp index 078598791..c8c351b06 100644 --- a/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp +++ b/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp @@ -164,7 +164,7 @@ namespace Gwen glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - GLenum format = GL_RGB; + //GLenum format = GL_RGB; unsigned char* texdata = new unsigned char[256*256*4]; for (int i=0;i<256*256;i++) { @@ -316,11 +316,11 @@ namespace Gwen Gwen::String converted_string = Gwen::Utility::UnicodeToString( text ); float yOffset=0.0f; - for ( int i=0; iDrawLinedRect( control->GetRenderBounds() ); } - Gwen::Rect rect = control->GetRenderBounds(); + // Gwen::Rect rect = control->GetRenderBounds(); if ( bChecked ) { m_Render->SetDrawColor( Color( 0, 0, 0, 255) ); @@ -490,7 +490,7 @@ namespace Gwen virtual void DrawSlider( Gwen::Controls::Base* control, bool bIsHorizontal, int numNotches, int barSize) { Gwen::Rect rect = control->GetRenderBounds(); - Gwen::Rect notchRect = rect; +// Gwen::Rect notchRect = rect; if ( bIsHorizontal ) { diff --git a/btgui/OpenGLTrueTypeFont/fontstash.cpp b/btgui/OpenGLTrueTypeFont/fontstash.cpp index 8a2bf3de8..56eaeb3a8 100644 --- a/btgui/OpenGLTrueTypeFont/fontstash.cpp +++ b/btgui/OpenGLTrueTypeFont/fontstash.cpp @@ -145,7 +145,7 @@ struct sth_stash* sth_create(int cachew, int cacheh, RenderCallbacks* renderCall memset(stash,0,sizeof(struct sth_stash)); stash->m_renderCallbacks = renderCallbacks; - + // Allocate memory for the first texture texture = (struct sth_texture*)malloc(sizeof(struct sth_texture)); if (texture == NULL) @@ -161,11 +161,11 @@ struct sth_stash* sth_create(int cachew, int cacheh, RenderCallbacks* renderCall stash->itw = 1.0f/cachew; stash->ith = 1.0f/cacheh; stash->textures = texture; - - stash->m_renderCallbacks->updateTexture(texture, 0, stash->tw, stash->th); - + + stash->m_renderCallbacks->updateTexture(texture, 0, stash->tw, stash->th); + return stash; - + } int sth_add_font_from_memory(struct sth_stash* stash, unsigned char* buffer) @@ -177,7 +177,7 @@ int sth_add_font_from_memory(struct sth_stash* stash, unsigned char* buffer) if (fnt == NULL) goto error; memset(fnt,0,sizeof(struct sth_font)); - + // Init hash lookup. for (i = 0; i < HASH_LUT_SIZE; ++i) fnt->lut[i] = -1; @@ -187,8 +187,8 @@ int sth_add_font_from_memory(struct sth_stash* stash, unsigned char* buffer) // Init stb_truetype if (!stbtt_InitFont(&fnt->font, fnt->data, 0)) goto error; - - + + // Store normalized line height. The real line height is got // by multiplying the lineh by font size. stbtt_GetFontVMetrics(&fnt->font, &ascent, &descent, &lineGap); @@ -203,7 +203,7 @@ int sth_add_font_from_memory(struct sth_stash* stash, unsigned char* buffer) fnt->next = stash->fonts; stash->fonts = fnt; - + return idx++; error: @@ -219,8 +219,8 @@ int sth_add_font(struct sth_stash* stash, const char* path) FILE* fp = 0; int datasize; unsigned char* data = NULL; - int idx; - + int idx=0; + // Read in the font data. fp = fopen(path, "rb"); if (!fp) goto error; @@ -229,11 +229,16 @@ int sth_add_font(struct sth_stash* stash, const char* path) fseek(fp,0,SEEK_SET); data = (unsigned char*)malloc(datasize); if (data == NULL) goto error; - fread(data, 1, datasize, fp); + int bytesRead; + bytesRead = fread(data, 1, datasize, fp); + if (bytesRead) + { + idx = sth_add_font_from_memory(stash, data); + } fclose(fp); fp = 0; + - idx = sth_add_font_from_memory(stash, data); // Modify type of the loaded font. if (idx) stash->fonts->type = TTFONT_FILE; @@ -241,7 +246,7 @@ int sth_add_font(struct sth_stash* stash, const char* path) free(data); return idx; - + error: if (data) free(data); if (fp) fclose(fp); @@ -271,7 +276,7 @@ int sth_add_bitmap_font(struct sth_stash* stash, int ascent, int descent, int li fnt->type = BMFONT; fnt->next = stash->fonts; stash->fonts = fnt; - + return idx++; error: @@ -295,7 +300,7 @@ error: if (stash == NULL) return; texture = stash->textures; - while (texture != NULL && texture->id != id) + while (texture != NULL && texture->id != id) texture = texture->next; if (texture == NULL) @@ -313,7 +318,7 @@ error: while (fnt != NULL && fnt->idx != idx) fnt = fnt->next; if (fnt == NULL) return; if (fnt->type != BMFONT) return; - + for (; *s; ++s) { if (!decutf8(&state, &codepoint, *(unsigned char*)s)) break; @@ -338,7 +343,7 @@ error: glyph->xoff = xoffset; glyph->yoff = yoffset - base; glyph->xadv = xadvance; - + // Find code point and size. h = hashint(codepoint) & (HASH_LUT_SIZE-1); // Insert char to hash lookup. @@ -353,7 +358,7 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt float scale; struct sth_texture* texture = NULL; struct sth_glyph* glyph = NULL; - + unsigned int h; float size = isize/10.0f; int rh; @@ -369,10 +374,10 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt i = fnt->glyphs[i].next; } // Could not find glyph. - + // For bitmap fonts: ignore this glyph. if (fnt->type == BMFONT) return 0; - + // For truetype fonts: create this glyph. scale = stbtt_ScaleForPixelHeight(&fnt->font, size); g = stbtt_FindGlyphIndex(&fnt->font, codepoint); @@ -380,7 +385,7 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt stbtt_GetGlyphBitmapBox(&fnt->font, g, scale,scale, &x0,&y0,&x1,&y1); gw = x1-x0; gh = y1-y0; - + // Check if glyph is larger than maximum texture size if (gw >= stash->tw || gh >= stash->th) return 0; @@ -396,7 +401,7 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt if (texture->rows[i].h >= rh && texture->rows[i].x+gw+1 <= stash->tw) br = &texture->rows[i]; } - + // If no row is found, there are 3 possibilities: // - add new row // - try next texture @@ -421,10 +426,10 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt texture = texture->next; if (texture == NULL) goto error; memset(texture,0,sizeof(struct sth_texture)); - + stash->m_renderCallbacks->updateTexture(texture,0,stash->tw,stash->th); - + } continue; } @@ -434,10 +439,10 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt br->x = BORDER_X_LEFT; br->y = py+BORDER_Y_BOTTOM; br->h = rh+ADDITIONAL_HEIGHT; - texture->nrows++; + texture->nrows++; } } - + // Alloc space for new glyph. fnt->nglyphs++; fnt->glyphs = (sth_glyph*)realloc(fnt->glyphs, fnt->nglyphs*sizeof(struct sth_glyph)); @@ -473,7 +478,7 @@ static struct sth_glyph* get_glyph(struct sth_stash* stash, struct sth_font* fnt stash->m_renderCallbacks->updateTexture(texture,glyph, stash->tw, stash->th); } - + return glyph; error: @@ -486,26 +491,26 @@ static int get_quad(struct sth_stash* stash, struct sth_font* fnt, struct sth_gl { float rx,ry; float scale = 1.f/s_retinaScale;//1.0f; - + if (fnt->type == BMFONT) scale = isize/(glyph->size*10.0f); rx = (*x + scale * float(glyph->xoff)); ry = (*y + scale * float(glyph->yoff)); - + q->x0 = rx; q->y0 = ry + 1.5f*0.5f*float(isize)/10.f; q->x1 = rx + scale * float(glyph->x1 - glyph->x0_); q->y1 = ry + scale * float(glyph->y1 - glyph->y0)+ 1.5f*0.5f*float(isize)/10.f; - + q->s0 = float(glyph->x0_) * stash->itw; q->t0 = float(glyph->y0) * stash->ith; q->s1 = float(glyph->x1) * stash->itw; q->t1 = float(glyph->y1) * stash->ith; - + *x += scale * glyph->xadv; - + return 1; } @@ -523,7 +528,7 @@ static Vertex* setv(Vertex* v, float x, float y, float s, float t, float width, } v->position.p[2] = 0.f; v->position.p[3] = 1.f; - + v->uv.p[0] = s; v->uv.p[1] = t; @@ -531,7 +536,7 @@ static Vertex* setv(Vertex* v, float x, float y, float s, float t, float width, v->colour.p[1] = 0.1f; v->colour.p[2] = 0.1f; v->colour.p[3] = 1.f; - + return v+1; } @@ -545,7 +550,7 @@ static void flush_draw(struct sth_stash* stash) { if (texture->nverts > 0) { - stash->m_renderCallbacks->render(texture); + stash->m_renderCallbacks->render(texture); texture->nverts = 0; } texture = texture->next; @@ -584,7 +589,7 @@ void sth_end_draw(struct sth_stash* stash) stash->nverts += 6; } */ - + flush_draw(stash); stash->drawing = 0; } @@ -606,7 +611,7 @@ void sth_draw_texture(struct sth_stash* stash, short isize = (short)(size*10.0f); Vertex* v; struct sth_font* fnt = NULL; - + if (stash == NULL) return; if (!stash->textures) return; @@ -614,23 +619,23 @@ void sth_draw_texture(struct sth_stash* stash, while(fnt != NULL && fnt->idx != idx) fnt = fnt->next; if (fnt == NULL) return; if (fnt->type != BMFONT && !fnt->data) return; - + int once = true; for (; once; ++s) { once=false; - if (decutf8(&state, &codepoint, *(unsigned char*)s)) + if (decutf8(&state, &codepoint, *(unsigned char*)s)) continue; glyph = get_glyph(stash, fnt, codepoint, isize); - if (!glyph) + if (!glyph) continue; texture = glyph->texture; if (texture->nverts+6 >= VERT_COUNT) flush_draw(stash); - - if (!get_quad(stash, fnt, glyph, isize, &x, &y, &q)) + + if (!get_quad(stash, fnt, glyph, isize, &x, &y, &q)) continue; - + v = &texture->newverts[texture->nverts]; q.x0 = 0; q.y0 = 0; @@ -646,11 +651,11 @@ void sth_draw_texture(struct sth_stash* stash, v = setv(v, q.x0, q.y1, 0,1,(float)screenwidth,(float)screenheight); texture->nverts += 6; } - + flush_draw(stash); - + if (dx) *dx = x; } @@ -664,7 +669,7 @@ void sth_draw_text(struct sth_stash* stash, float x, float y, const char* s, float* dx, int screenwidth, int screenheight, int measureOnly, float retinaScale) { - + unsigned int codepoint; struct sth_glyph* glyph = NULL; struct sth_texture* texture = NULL; @@ -673,7 +678,7 @@ void sth_draw_text(struct sth_stash* stash, short isize = (short)(size*10.0f); Vertex* v; struct sth_font* fnt = NULL; - + s_retinaScale = retinaScale; if (stash == NULL) return; @@ -682,23 +687,23 @@ void sth_draw_text(struct sth_stash* stash, while(fnt != NULL && fnt->idx != idx) fnt = fnt->next; if (fnt == NULL) return; if (fnt->type != BMFONT && !fnt->data) return; - + for (; *s; ++s) { - if (decutf8(&state, &codepoint, *(unsigned char*)s)) + if (decutf8(&state, &codepoint, *(unsigned char*)s)) continue; glyph = get_glyph(stash, fnt, codepoint, isize); if (!glyph) continue; texture = glyph->texture; - + if (!measureOnly) { if (texture->nverts+6 >= VERT_COUNT) flush_draw(stash); } - + if (!get_quad(stash, fnt, glyph, isize, &x, &y, &q)) continue; - + if (!measureOnly) { v = &texture->newverts[texture->nverts]; @@ -714,7 +719,7 @@ void sth_draw_text(struct sth_stash* stash, texture->nverts += 6; } } - + if (dx) *dx = x; } @@ -730,16 +735,16 @@ void sth_dim_text(struct sth_stash* stash, short isize = (short)(size*10.0f); struct sth_font* fnt = NULL; float x = 0, y = 0; - - if (stash == NULL) + + if (stash == NULL) return; - if (!stash->textures) + if (!stash->textures) return; fnt = stash->fonts; while(fnt != NULL && fnt->idx != idx) fnt = fnt->next; if (fnt == NULL) return; if (fnt->type != BMFONT && !fnt->data) return; - + *minx = *maxx = x; *miny = *maxy = y; diff --git a/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp b/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp index d15485219..35250ce2e 100644 --- a/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp +++ b/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp @@ -37,8 +37,7 @@ InternalOpenGL2RenderCallbacks::~InternalOpenGL2RenderCallbacks() void InternalOpenGL2RenderCallbacks::display2() { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // glViewport(0,0,10,10); //const float timeScale = 0.008f; @@ -48,39 +47,31 @@ void InternalOpenGL2RenderCallbacks::display2() glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBindVertexArray(s_vertexArrayObject); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + // glBindTexture(GL_TEXTURE_2D,m_texturehandle); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); glUniform2fv(data->m_positionUniform, 1, (const GLfloat *)&p); - err = glGetError(); - assert(err==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + glEnableVertexAttribArray(data->m_positionAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(data->m_textureAttribute); glVertexAttribPointer(data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); glVertexAttribPointer(data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); glVertexAttribPointer(data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); /* glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_indexBuffer); @@ -101,10 +92,7 @@ void InternalOpenGL2RenderCallbacks::display2() void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_glyph* glyph, int textureWidth, int textureHeight) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); if (glyph) @@ -114,13 +102,11 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glBindTexture(GL_TEXTURE_2D, *gltexture); glPixelStorei(GL_UNPACK_ALIGNMENT,1); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels); - GLenum err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } else { @@ -132,8 +118,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly //create new texture glGenTextures(1, texId); - GLenum err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); @@ -143,8 +128,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); //////////////////////////// //create the other data @@ -155,8 +139,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glGenBuffers(1, &s_vertexBuffer); glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBufferData(GL_ARRAY_BUFFER, VERT_COUNT * sizeof(Vertex), texture->newverts, GL_DYNAMIC_DRAW); - GLuint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); for (int i=0;im_userData; - GLint err; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindTexture(GL_TEXTURE_2D, *texId); bool useFiltering = false; @@ -215,25 +194,21 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture) glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); } - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBindVertexArray(s_vertexArrayObject); glBufferData(GL_ARRAY_BUFFER, texture->nverts * sizeof(Vertex), &texture->newverts[0].position.p[0], GL_DYNAMIC_DRAW); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s_indexBuffer); //glDrawArrays(GL_TRIANGLE_FAN, 0, 4); int indexCount = texture->nverts; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindVertexArray(0); diff --git a/btgui/OpenGLWindow/CMakeLists.txt b/btgui/OpenGLWindow/CMakeLists.txt index 4af098ac0..8faf453c7 100644 --- a/btgui/OpenGLWindow/CMakeLists.txt +++ b/btgui/OpenGLWindow/CMakeLists.txt @@ -32,7 +32,14 @@ ENDIF(APPLE) #no Linux detection? IF(NOT WIN32 AND NOT APPLE) - SET(OpenGLWindow_SRCS ${OpenGLWindowLinux_CPP} ${OpenGLWindowCommon_CPP} ) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/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} ) ENDIF() diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 30a901136..dd1d8cf2d 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -16,8 +16,8 @@ subject to the following restrictions: ///todo: make this configurable in the gui bool useShadowMap=true;//false;//true; -float shadowMapWidth=8192; -float shadowMapHeight=8192; +int shadowMapWidth=8192; +int shadowMapHeight=8192; float shadowMapWorldSize=100; float WHEEL_MULTIPLIER=0.01f; float MOUSE_MOVE_MULTIPLIER = 0.4f; diff --git a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp index e1c424659..ceea63ee8 100644 --- a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -131,8 +131,8 @@ 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); - GLuint err = glGetError(); - assert(err==GL_NO_ERROR); + + assert(glGetError()==GL_NO_ERROR); @@ -142,16 +142,14 @@ void GLPrimitiveRenderer::loadBufferData() glEnableVertexAttribArray(m_data->m_positionAttribute); glEnableVertexAttribArray(m_data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); 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))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); @@ -191,8 +189,7 @@ void GLPrimitiveRenderer::loadBufferData() glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image); glGenerateMipmap(GL_TEXTURE_2D); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); delete[] image; @@ -217,36 +214,25 @@ void GLPrimitiveRenderer::drawLine() void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4]) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glUseProgram(m_data->m_shaderProg); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); glBindVertexArray(m_data->m_vertexArrayObject); @@ -263,10 +249,10 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y } Vertex vertexData[4] = { - { vec4(-1.+2.*x0/float(m_screenWidth), 1.-2.*y0/float(m_screenHeight), 0, 0 ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v0)}, - { vec4(-1.+2.*x0/float(m_screenWidth), 1.-2.*y1/float(m_screenHeight), 0, 1 ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v1)}, - { vec4( -1.+2.*x1/float(m_screenWidth), 1.-2.*y1/float(m_screenHeight), 1, 1 ), vec4(color[0], color[1], color[2], color[3]) ,vec2(u1,v1)}, - { vec4( -1.+2.*x1/float(m_screenWidth), 1.-2.*y0/float(m_screenHeight), 1, 0 ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u1,v0)} + { 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)} }; glBufferSubData(GL_ARRAY_BUFFER, 0,4 * sizeof(Vertex), vertexData); @@ -276,8 +262,7 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); if (useRGBA) @@ -290,60 +275,47 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_positionAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); 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))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer); //glDrawArrays(GL_TRIANGLE_FAN, 0, 4); int indexCount = 6; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindVertexArray(0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); //glDisableVertexAttribArray(m_data->m_textureAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glUseProgram(0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } diff --git a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h index 630f20c38..e4357bd7a 100644 --- a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h +++ b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h @@ -119,55 +119,37 @@ public: { m_yOffset=0; glEnable(GL_BLEND); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDisable(GL_DEPTH_TEST); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); //glColor4ub(255,0,0,255); - err = glGetError(); - assert(err==GL_NO_ERROR); - - err = glGetError(); - assert(err==GL_NO_ERROR); + glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); // saveOpenGLState(width,height);//m_glutScreenWidth,m_glutScreenHeight); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); glDisable(GL_CULL_FACE); glDisable(GL_DEPTH_TEST); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); - + glEnable(GL_BLEND); - - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); } virtual void End() { @@ -375,10 +357,7 @@ public: glBindTexture(GL_TEXTURE_2D,texHandle); // glDisable(GL_DEPTH_TEST); - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); /* bool useFiltering = true; @@ -406,8 +385,8 @@ public: m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, color,0+add,0,1+add,1,true); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); + } diff --git a/btgui/OpenGLWindow/LoadShader.cpp b/btgui/OpenGLWindow/LoadShader.cpp index efef4550a..e24fa7f12 100644 --- a/btgui/OpenGLWindow/LoadShader.cpp +++ b/btgui/OpenGLWindow/LoadShader.cpp @@ -18,8 +18,7 @@ void gltLoadShaderSrc(const char *szShaderSrc, GLuint shader) GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) { - GLuint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // Temporary Shader objects GLuint hVertexShader; @@ -36,7 +35,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) // Compile them glCompileShader(hVertexShader); - err = glGetError(); + assert(glGetError()==GL_NO_ERROR); glGetShaderiv(hVertexShader, GL_COMPILE_STATUS, &testVal); if(testVal == GL_FALSE) @@ -51,10 +50,10 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) return (GLuint)NULL; } - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glCompileShader(hFragmentShader); - err = glGetError(); + assert(glGetError()==GL_NO_ERROR); glGetShaderiv(hFragmentShader, GL_COMPILE_STATUS, &testVal); if(testVal == GL_FALSE) @@ -69,7 +68,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) return (GLuint)NULL; } - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // Check for errors diff --git a/btgui/OpenGLWindow/MacOpenGLWindow.h b/btgui/OpenGLWindow/MacOpenGLWindow.h index ac92f1509..179b1efe2 100644 --- a/btgui/OpenGLWindow/MacOpenGLWindow.h +++ b/btgui/OpenGLWindow/MacOpenGLWindow.h @@ -81,7 +81,7 @@ public: virtual void setWindowTitle(const char* title); - + int fileOpenDialog(char* filename, int maxNameLength); }; diff --git a/btgui/OpenGLWindow/MacOpenGLWindow.mm b/btgui/OpenGLWindow/MacOpenGLWindow.mm index 8cab0de5e..cd77a9461 100644 --- a/btgui/OpenGLWindow/MacOpenGLWindow.mm +++ b/btgui/OpenGLWindow/MacOpenGLWindow.mm @@ -989,8 +989,48 @@ void MacOpenGLWindow::setRequestExit() m_internalData->m_exitRequested = true; } +#include +int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) +{ + //save/restore the OpenGL context, NSOpenPanel can mess it up + //http://stackoverflow.com/questions/13987148/nsopenpanel-breaks-my-sdl-opengl-app + + NSOpenGLContext *foo = [NSOpenGLContext currentContext]; + // get the url of a .txt file + NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel]; + NSArray * zAryOfExtensions = [NSArray arrayWithObject:@"urdf"]; + [zOpenPanel setAllowedFileTypes:zAryOfExtensions]; + NSInteger zIntResult = [zOpenPanel runModal]; + + [foo makeCurrentContext]; + + if (zIntResult == NSFileHandlingPanelCancelButton) { + NSLog(@"readUsingOpenPanel cancelled"); + return 0; + } + NSURL *zUrl = [zOpenPanel URL]; + if (zUrl) + { + //without the file:// + NSString *myString = [zUrl absoluteString]; + int slen = [myString length]; + if (slen < maxNameLength) + { + const char *cfilename=[myString UTF8String]; + //expect file:// at start of URL + const char* p = strstr(cfilename, "file://"); + if (p==cfilename) + { + int actualLen = strlen(cfilename)-7; + memcpy(filename, cfilename+7,actualLen); + filename[actualLen]=0; + return actualLen; + } + } + } - + return 0; +} diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp index b0b8c987c..e9c38d783 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp @@ -430,19 +430,17 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char* { int numComponents = 4; //glPixelStorei(GL_PACK_ALIGNMENT,1); - GLuint err=glGetError(); - assert(err==GL_NO_ERROR); + + b3Assert(glGetError()==GL_NO_ERROR); //glReadBuffer(GL_BACK);//COLOR_ATTACHMENT0); - err=glGetError(); - assert(err==GL_NO_ERROR); + float* orgPixels = (float*)malloc(textureWidth*textureHeight*numComponents*4); glReadPixels(0,0,textureWidth, textureHeight, GL_RGBA, GL_FLOAT, orgPixels); //it is useful to have the actual float values for debugging purposes //convert float->char char* pixels = (char*)malloc(textureWidth*textureHeight*numComponents); - err=glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); for (int j=0;j -GLint att[] = { GLX_RGBA, GLX_DEPTH_SIZE, 24, GLX_DOUBLEBUFFER, None }; +GLint att[] = { GLX_RGBA, +GLX_DEPTH_SIZE, 24, +GLX_RED_SIZE , 8, +GLX_GREEN_SIZE , 8, +GLX_BLUE_SIZE , 8, +GLX_ALPHA_SIZE , 8, +GLX_STENCIL_SIZE , 8, +GLX_DOUBLEBUFFER, +None }; +/* + static int att[] = + { + GLX_RGBA, GLX_DEPTH_SIZE, 24, GLX_DOUBLEBUFFER, None + + GLX_X_RENDERABLE , True, + GLX_DRAWABLE_TYPE , GLX_WINDOW_BIT, + GLX_RENDER_TYPE , GLX_RGBA_BIT, + GLX_X_VISUAL_TYPE , GLX_TRUE_COLOR, + GLX_RED_SIZE , 8, + GLX_GREEN_SIZE , 8, + GLX_BLUE_SIZE , 8, + GLX_ALPHA_SIZE , 8, + GLX_DEPTH_SIZE , 24, + GLX_STENCIL_SIZE , 8, + GLX_DOUBLEBUFFER , True, + None + }; +*/ static bool forceOpenGL3 = true; @@ -37,6 +64,7 @@ static bool forceOpenGL3 = true; #ifdef DYNAMIC_LOAD_X11_FUNCTIONS ///our X11 function typedefs + typedef int (*PFNXFREE)(void*); typedef XErrorHandler (* PFNXSETERRORHANDLER) (XErrorHandler); typedef int (* PFNXSYNC) (Display* a,Bool b); @@ -47,10 +75,14 @@ typedef int (*PFNXMAPWINDOW) (Display*, Window); typedef int (*PFNXSTORENAME) (Display* a,Window b,_Xconst char* c); typedef int (*PFNXCLOSEDISPLAY) (Display* a); typedef int (*PFNXDESTROYWINDOW) (Display* a,Window b); +typedef int (*PFNXRAISEWINDOW) (Display* a, Window b); + #if NeedWidePrototypes -typedef KeySym (*PFNXKEYCODETOKEYSYM) (Display* a,unsigned int b,int c); + typedef KeySym* (*PFNXGETKEYBOARDMAPPING) (Display*,unsigned int,int,int*); + typedef KeySym (*PFNXKEYCODETOKEYSYM) (Display* a,unsigned int b,int c); #else -typedef KeySym (*PFNXKEYCODETOKEYSYM) (Display* a,KeyCode b,int c); + typedef KeySym* (*PFNXGETKEYBOARDMAPPING) (Display*,KeyCode,int,int*); + typedef KeySym (*PFNXKEYCODETOKEYSYM) (Display* a,KeyCode b,int c); #endif typedef void (*PFNXCONVERTCASE) (KeySym /* sym */,KeySym * /* lower */,KeySym * /* upper */); typedef int (*PFNXPENDING) (Display* a); @@ -63,6 +95,7 @@ typedef Status (*PFNXGETWINDOWATTRIBUTES) (Display* a,Window b,XWindowAttributes #define X11_LIBRARY "libX11.so.6" #define MyXSync m_data->m_x11_XSync +#define MyXGetKeyboardMapping m_data->m_x11_XGetKeyboardMapping #define MyXSetErrorHandler m_data->m_x11_XSetErrorHandler #define MyXOpenDisplay m_data->m_x11_XOpenDisplay #define MyXCreateColormap m_data->m_x11_XCreateColormap @@ -70,6 +103,7 @@ typedef Status (*PFNXGETWINDOWATTRIBUTES) (Display* a,Window b,XWindowAttributes #define MyXMapWindow m_data->m_x11_XMapWindow #define MyXStoreName m_data->m_x11_XStoreName #define MyXDestroyWindow m_data->m_x11_XDestroyWindow +#define MyXRaiseWindow m_data->m_x11_XRaiseWindow #define MyXCloseDisplay m_data->m_x11_XCloseDisplay #define MyXKeycodeToKeysym m_data->m_x11_XKeycodeToKeysym #define MyXConvertCase m_data->m_x11_XConvertCase @@ -87,6 +121,7 @@ typedef Status (*PFNXGETWINDOWATTRIBUTES) (Display* a,Window b,XWindowAttributes #else #define MyXSync XSync +#define MyXGetKeyboardMapping XGetKeyboardMapping #define MyXSetErrorHandler XSetErrorHandler #define MyXOpenDisplay XOpenDisplay #define MyXCreateColormap XCreateColormap @@ -94,6 +129,7 @@ typedef Status (*PFNXGETWINDOWATTRIBUTES) (Display* a,Window b,XWindowAttributes #define MyXMapWindow XMapWindow #define MyXStoreName XStoreName #define MyXDestroyWindow XDestroyWindow +#define MyXRaiseWindow XRaiseWindow #define MyXCloseDisplay XCloseDisplay #define MyXKeycodeToKeysym XKeycodeToKeysym #define MyXConvertCase XConvertCase @@ -136,7 +172,9 @@ struct InternalData2 PFNXSTORENAME m_x11_XStoreName; PFNXCLOSEDISPLAY m_x11_XCloseDisplay; PFNXDESTROYWINDOW m_x11_XDestroyWindow; + PFNXRAISEWINDOW m_x11_XRaiseWindow; PFNXKEYCODETOKEYSYM m_x11_XKeycodeToKeysym; + PFNXGETKEYBOARDMAPPING m_x11_XGetKeyboardMapping; PFNXCONVERTCASE m_x11_XConvertCase; PFNXPENDING m_x11_XPending; PFNXNEXTEVENT m_x11_XNextEvent; @@ -190,13 +228,15 @@ struct InternalData2 if (missingFunc) { printf("Error: missing func XMapWindow in %s, exiting!\n", X11_LIBRARY); exit(0);} missingFunc = ((m_x11_XStoreName = (PFNXSTORENAME) dlsym(m_x11_library,"XStoreName"))==NULL) | missingFunc; if (missingFunc) { printf("Error: missing func XStoreName in %s, exiting!\n", X11_LIBRARY); exit(0);} - missingFunc = ((m_x11_XCloseDisplay = (PFNXCLOSEDISPLAY) dlsym(m_x11_library,"XCloseDisplay"))==NULL) | missingFunc; if (missingFunc) { printf("Error: missing func XCloseDisplay in %s, exiting!\n", X11_LIBRARY); exit(0);} - missingFunc = ((m_x11_XDestroyWindow = (PFNXDESTROYWINDOW) dlsym(m_x11_library,"XDestroyWindow"))==NULL) | missingFunc; - if (missingFunc) { printf("Error: missing func XMapWindow in %s, exiting!\n", X11_LIBRARY); exit(0);} + if (missingFunc) { printf("Error: missing func XDestroyWindow in %s, exiting!\n", X11_LIBRARY); exit(0);} + missingFunc = ((m_x11_XRaiseWindow = (PFNXRAISEWINDOW) dlsym(m_x11_library,"XRaiseWindow"))==NULL) | missingFunc; + if (missingFunc) { printf("Error: missing func XRaiseWindow in %s, exiting!\n", X11_LIBRARY); exit(0);} + missingFunc = ((m_x11_XGetKeyboardMapping = (PFNXGETKEYBOARDMAPPING) dlsym(m_x11_library,"XGetKeyboardMapping"))==NULL) | missingFunc; + if (missingFunc) { printf("Error: missing func XGetKeyboardMapping in %s, exiting!\n", X11_LIBRARY); exit(0);} missingFunc = ((m_x11_XKeycodeToKeysym = (PFNXKEYCODETOKEYSYM) dlsym(m_x11_library,"XKeycodeToKeysym"))==NULL) | missingFunc; if (missingFunc) { printf("Error: missing func XKeycodeToKeysym in %s, exiting!\n", X11_LIBRARY); exit(0);} missingFunc = ((m_x11_XConvertCase = (PFNXCONVERTCASE) dlsym(m_x11_library,"XConvertCase"))==NULL) | missingFunc; @@ -612,9 +652,21 @@ void X11OpenGLWindow::closeWindow() int X11OpenGLWindow::getAsciiCodeFromVirtualKeycode(int keycode) { + int result = 0; + KeySym key, key_lc, key_uc; - key = MyXKeycodeToKeysym( m_data->m_dpy, keycode, 0 ); + int keysyms_per_keycode_return; + KeySym *keysym = MyXGetKeyboardMapping(m_data->m_dpy, + keycode, + 1, + &keysyms_per_keycode_return); + + key = keysym[0]; + + + //key = MyXKeycodeToKeysym( m_data->m_dpy, keycode, 0 ); + switch( key ) { case XK_Escape: return B3G_ESCAPE; @@ -660,9 +712,12 @@ int X11OpenGLWindow::getAsciiCodeFromVirtualKeycode(int keycode) { return (int) key; } - return -1; + result = -1; } - return 0; + + MyXFree(keysym); + + return result; } void X11OpenGLWindow::pumpMessage() @@ -913,4 +968,29 @@ b3KeyboardCallback X11OpenGLWindow::getKeyboardCallback() { return m_data->m_keyboardCallback; } +#include +int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) +{ + int len = 0; + FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.*\"","r"); + if (output) + { + while( fgets(filename, maxNameLength-1, output) != NULL ) + { + len=strlen(filename); + if (len>0) + { + filename[len-1]=0; + printf("file open (length=%d) = %s\n", len,filename); + } + } + pclose(output); + } else + { + printf("Error: fileOpenDialog no popen output, perhaps install zenity?\n"); + } + MyXRaiseWindow(m_data->m_dpy, m_data->m_win); + return len; + +} diff --git a/btgui/OpenGLWindow/X11OpenGLWindow.h b/btgui/OpenGLWindow/X11OpenGLWindow.h index 1714fa2d1..14099423d 100644 --- a/btgui/OpenGLWindow/X11OpenGLWindow.h +++ b/btgui/OpenGLWindow/X11OpenGLWindow.h @@ -59,6 +59,7 @@ public: virtual void setWindowTitle(const char* title); + int fileOpenDialog(char* filename, int maxNameLength); }; diff --git a/btgui/OpenGLWindow/b3gWindowInterface.h b/btgui/OpenGLWindow/b3gWindowInterface.h index e404e4629..787f4214d 100644 --- a/btgui/OpenGLWindow/b3gWindowInterface.h +++ b/btgui/OpenGLWindow/b3gWindowInterface.h @@ -110,6 +110,8 @@ class b3gWindowInterface virtual float getRetinaScale() const =0; + virtual int fileOpenDialog(char* fileName, int maxFileNameLength) = 0; + }; #endif //B3G_WINDOW_INTERFACE_H \ No newline at end of file diff --git a/btgui/lua-5.2.3/premake4.lua b/btgui/lua-5.2.3/premake4.lua index 1c66da246..2a1e4aeae 100644 --- a/btgui/lua-5.2.3/premake4.lua +++ b/btgui/lua-5.2.3/premake4.lua @@ -5,8 +5,14 @@ --flags {} - defines { "LUA_COMPAT_ALL"} + + if os.is("Linux") then + defines {"LUA_USE_LINUX"} + end + if os.is("MacOSX") then + defines {"LUA_USE_MACOSX"} + end targetdir "../../lib" includedirs { @@ -18,4 +24,4 @@ } include "lua_compiler" -include "lua_standalone" \ No newline at end of file +include "lua_standalone" diff --git a/btgui/lua-5.2.3/src/loadlib.c b/btgui/lua-5.2.3/src/loadlib.c index bedbea3e9..9318a9980 100644 --- a/btgui/lua-5.2.3/src/loadlib.c +++ b/btgui/lua-5.2.3/src/loadlib.c @@ -115,6 +115,7 @@ static lua_CFunction ll_sym (lua_State *L, void *lib, const char *sym); #if defined(LUA_USE_DLOPEN) + /* ** {======================================================================== ** This is an implementation of loadlib based on the dlfcn interface. diff --git a/btgui/lua-5.2.3/src/luaconf.h b/btgui/lua-5.2.3/src/luaconf.h index 18be9a9e4..e8aee009a 100644 --- a/btgui/lua-5.2.3/src/luaconf.h +++ b/btgui/lua-5.2.3/src/luaconf.h @@ -42,8 +42,8 @@ #if defined(LUA_USE_LINUX) #define LUA_USE_POSIX -#define LUA_USE_DLOPEN /* needs an extra library: -ldl */ -#define LUA_USE_READLINE /* needs some extra libraries */ +//#define LUA_USE_DLOPEN /* needs an extra library: -ldl */ +//#define LUA_USE_READLINE /* needs some extra libraries */ #define LUA_USE_STRTODHEX /* assume 'strtod' handles hex formats */ #define LUA_USE_AFORMAT /* assume 'printf' handles 'aA' specifiers */ #define LUA_USE_LONGLONG /* assume support for long long */ @@ -51,8 +51,8 @@ #if defined(LUA_USE_MACOSX) #define LUA_USE_POSIX -#define LUA_USE_DLOPEN /* does not need -ldl */ -#define LUA_USE_READLINE /* needs an extra library: -lreadline */ +//#define LUA_USE_DLOPEN /* does not need -ldl */ +//#define LUA_USE_READLINE /* needs an extra library: -lreadline */ #define LUA_USE_STRTODHEX /* assume 'strtod' handles hex formats */ #define LUA_USE_AFORMAT /* assume 'printf' handles 'aA' specifiers */ #define LUA_USE_LONGLONG /* assume support for long long */ diff --git a/btgui/minizip/crypt.h b/btgui/minizip/crypt.h index a01d08d93..acdf125b9 100644 --- a/btgui/minizip/crypt.h +++ b/btgui/minizip/crypt.h @@ -26,6 +26,8 @@ http://www.winzip.com/aes_info.htm ) and PKWare PKZip 5.x Strong Encryption is not supported. */ +#define NOCRYPT +#define NOUNCRYPT #define CRC32(c, b) ((*(pcrc_32_tab+(((int)(c) ^ (b)) & 0xff))) ^ ((c) >> 8)) diff --git a/btgui/urdf/urdfdom/urdf_parser/src/model.cpp b/btgui/urdf/urdfdom/urdf_parser/src/model.cpp index 7c9d7e9c7..e8562d09b 100644 --- a/btgui/urdf/urdfdom/urdf_parser/src/model.cpp +++ b/btgui/urdf/urdfdom/urdf_parser/src/model.cpp @@ -116,6 +116,7 @@ my_shared_ptr parseURDF(const std::string &xml_string) { my_shared_ptr link; link.reset(new Link); + model->m_numLinks++; try { parseLink(*link, link_xml); @@ -176,6 +177,7 @@ my_shared_ptr parseURDF(const std::string &xml_string) { my_shared_ptr joint; joint.reset(new Joint); + model->m_numJoints++; if (parseJoint(*joint, joint_xml)) { diff --git a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h index 67a1647ec..22e64cf5a 100644 --- a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h +++ b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h @@ -223,6 +223,8 @@ public: std::vector > child_joints; std::vector > child_links; + mutable int m_link_index; + const Link* getParent() const {return parent_link_;} @@ -242,6 +244,8 @@ public: this->child_links.clear(); this->collision_array.clear(); this->visual_array.clear(); + this->m_link_index=-1; + this->parent_link_ = NULL; }; private: diff --git a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h index 5bd11a6d6..8e93d94e4 100644 --- a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h +++ b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h @@ -41,7 +41,7 @@ #include //#include #include - +#include //printf #include namespace urdf { @@ -82,6 +82,8 @@ public: void clear() { + m_numLinks=0; + m_numJoints = 0; name_.clear(); this->links_.clear(); this->joints_.clear(); @@ -132,6 +134,7 @@ public: this->getLink(child_link_name, child_link); if (!child_link) { + printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() ); assert(0); // throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); } @@ -206,6 +209,8 @@ public: /// \brief The root is always a link (the parent of the tree describing the robot) my_shared_ptr root_link_; + int m_numLinks;//includes parent + int m_numJoints; }; diff --git a/build3/findOpenGLGlewGlut.lua b/build3/findOpenGLGlewGlut.lua index d2b495a00..e60a1d90d 100644 --- a/build3/findOpenGLGlewGlut.lua +++ b/build3/findOpenGLGlewGlut.lua @@ -1,3 +1,4 @@ + function findOpenGL() configuration{} if os.is("Linux") then @@ -7,6 +8,22 @@ return true end + function findOpenGL3() + configuration{} + if os.is("MacOSX") then + local osversion = os.getversion() + --Mac OSX 10.9 and above supports OpenGL 3, below doesn't, so ... + if osversion.majorversion > 10 or (osversion.majorversion == 10 and osversion.minorversion >=9) then + return findOpenGL() + else + return false + end + else + return findOpenGL() + end + end + + function initOpenGL() configuration {} configuration {"Windows"} diff --git a/build3/premake4.lua b/build3/premake4.lua index e0b9c72fe..97c092c82 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -1,6 +1,12 @@ solution "0_Bullet3Solution" + local osversion = os.getversion() + print(string.format(" %d.%d.%d (%s)", + osversion.majorversion, osversion.minorversion, osversion.revision, + osversion.description)) + + -- Multithreaded compiling if _ACTION == "vs2010" or _ACTION=="vs2008" then buildoptions { "/MP" } @@ -35,8 +41,8 @@ newoption { - trigger = "bullet2gpu", - description = "Enable Bullet 2.x GPU using b3GpuDynamicsWorld bridge to Bullet 3.x" + trigger = "bullet2demos", + description = "Compile the Bullet 2 demos (Demo/Extra folder)" } newoption @@ -113,13 +119,12 @@ include "../test/TestBullet3OpenCL" end -if findOpenGL() then +if findOpenGL3() then include "../Demos3/AllBullet2Demos" include "../Demos3/GpuDemos" include"../Demos3/BasicDemoConsole" include"../Demos3/BasicDemoCustomOpenGL2" - - + include "../btgui/OpenGLTrueTypeFont" -- include "../Demos3/CpuDemos" -- include "../Demos3/Wavefront" @@ -137,6 +142,7 @@ if findOpenGL() then include "../btgui/GwenOpenGLTest" end + -- include "../demo/gpudemo" if _OPTIONS["midi"] then include "../btgui/MidiTest" @@ -169,7 +175,7 @@ end -- include "../demo/gpu_initialize" -- include "../opencl/lds_bank_conflict" -- include "../opencl/reduce" - include "../btgui/OpenGLTrueTypeFont" +-- include "../btgui/OpenGLTrueTypeFont" -- include "../btgui/OpenGLWindow" -- include "../demo/ObjLoader" -- include "../test/b3DynamicBvhBroadphase" @@ -180,7 +186,10 @@ end include "../test/enet/client" end - - if _OPTIONS["bullet2gpu"] then + if _OPTIONS["bullet2demos"] then + include "../Extras" + if findOpenGL() then + include "../Demos" + end end diff --git a/data/r2d2.urdf b/data/r2d2.urdf new file mode 100644 index 000000000..aa6b9aede --- /dev/null +++ b/data/r2d2.urdf @@ -0,0 +1,414 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/spider.bullet b/data/spider.bullet new file mode 100644 index 000000000..05d17c886 Binary files /dev/null and b/data/spider.bullet differ diff --git a/src/Bullet3Common/b3FileUtils.h b/src/Bullet3Common/b3FileUtils.h new file mode 100644 index 000000000..348b34ffd --- /dev/null +++ b/src/Bullet3Common/b3FileUtils.h @@ -0,0 +1,116 @@ +#ifndef B3_FILE_UTILS_H +#define B3_FILE_UTILS_H + +#include +#include "b3Scalar.h" +#include //ptrdiff_h + +struct b3FileUtils +{ + b3FileUtils() + { + } + virtual ~b3FileUtils() + { + } + + bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + FILE* f=0; + f = fopen(orgFileName,"rb"); + if (f) + { + //printf("original file found: [%s]\n", orgFileName); + sprintf(relativeFileName,"%s", orgFileName); + fclose(f); + return true; + } + + //printf("Trying various directories, relative to current working directory\n"); + const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; + int numPrefixes = sizeof(prefix)/sizeof(const char*); + + f=0; + bool fileFound = false; + int result = 0; + + for (int i=0;!f && i0); + if (maxPathLength>0) + { + path[0] = 0; + } + } + } + + /*static const char* strip2(const char* name, const char* pattern) + { + size_t const patlen = strlen(pattern); + size_t patcnt = 0; + const char * oriptr; + const char * patloc; + // find how many times the pattern occurs in the original string + for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen) + { + patcnt++; + } + return oriptr; + } + */ + +}; +#endif //B3_FILE_UTILS_H diff --git a/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp b/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp index deb6d89d9..77248d113 100644 --- a/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp +++ b/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp @@ -276,8 +276,9 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs) { b3AlignedObjectArray pairsCpu; m_gpuPairs.copyToHost(pairsCpu); - - printf("m_gpuPairs.size()=%d\n",m_gpuPairs.size()); + + int sz = m_gpuPairs.size(); + printf("m_gpuPairs.size()=%d\n",sz); for (int i=0;i 0) { - if (strncmp((tempBuffer + ChunkUtils::getOffset(mFlags)), "SDNANAME", 8) ==0) + if (strncmp((tempBuffer + ChunkUtils::getOffset(mFlags)), "SDNANAME", 8) ==0) dna.oldPtr = (tempBuffer + ChunkUtils::getOffset(mFlags)); else dna.oldPtr = 0; } else dna.oldPtr = 0; - } + } // Some Bullet files are missing the DNA1 block // In Blender it's DNA1 + ChunkUtils::getOffset() + SDNA + NAME // In Bullet tests its SDNA + NAME @@ -224,7 +225,7 @@ void bFile::parseInternal(int verboseMode, char* memDna,int memDnaLength) dna.oldPtr = blenderData + i; dna.len = mFileLen-i; - // Also no REND block, so exit now. + // Also no REND block, so exit now. if (mVersion==276) break; } @@ -240,11 +241,11 @@ void bFile::parseInternal(int verboseMode, char* memDna,int memDnaLength) mFileDNA = new bDNA(); - - + + ///mFileDNA->init will convert part of DNA file endianness to current CPU endianness if necessary mFileDNA->init((char*)dna.oldPtr, dna.len, (mFlags & FD_ENDIAN_SWAP)!=0); - + if (mVersion==276) { @@ -270,10 +271,10 @@ void bFile::parseInternal(int verboseMode, char* memDna,int memDnaLength) mMemoryDNA = new bDNA(); int littleEndian= 1; littleEndian= ((char*)&littleEndian)[0]; - + mMemoryDNA->init(memDna,memDnaLength,littleEndian==0); - + ///@todo we need a better version check, add version/sub version info from FileGlobal into memory DNA/header files @@ -289,16 +290,16 @@ void bFile::parseInternal(int verboseMode, char* memDna,int memDnaLength) //printf ("Warning, file DNA is newer than built in."); } - + mFileDNA->initCmpFlags(mMemoryDNA); - + parseData(); - + resolvePointers(verboseMode); - + updateOldPointers(); - - + + } @@ -309,8 +310,8 @@ void bFile::swap(char *head, bChunkInd& dataChunk, bool ignoreEndianFlag) char *data = head; short *strc = mFileDNA->getStruct(dataChunk.dna_nr); - - + + const char s[] = "SoftBodyMaterialData"; int szs = sizeof(s); if (strncmp((char*)&dataChunk.code,"ARAY",4)==0) @@ -322,7 +323,7 @@ void bFile::swap(char *head, bChunkInd& dataChunk, bool ignoreEndianFlag) return; } } - + int len = mFileDNA->getLength(strc[0]); @@ -410,11 +411,11 @@ void bFile::swapDNA(char* ptr) // Parse names - if (swap) + if (swap) dataLen = ChunkUtils::swapInt(*intPtr); - else + else dataLen = *intPtr; - + *intPtr = ChunkUtils::swapInt(*intPtr); intPtr++; @@ -426,7 +427,7 @@ void bFile::swapDNA(char* ptr) cp++; } - + { nr= (long)cp; //long mask=3; @@ -448,9 +449,9 @@ void bFile::swapDNA(char* ptr) intPtr = (int*)cp; assert(strncmp(cp, "TYPE", 4)==0); intPtr++; - if (swap) + if (swap) dataLen = ChunkUtils::swapInt(*intPtr); - else + else dataLen = *intPtr; *intPtr = ChunkUtils::swapInt(*intPtr); @@ -485,7 +486,7 @@ void bFile::swapDNA(char* ptr) intPtr = (int*)cp; assert(strncmp(cp, "TLEN", 4)==0); intPtr++; - + shtPtr = (short*)intPtr; for ( i=0; i=0) { @@ -632,7 +633,7 @@ void bFile::preSwap() } - + } @@ -644,7 +645,7 @@ char* bFile::readStruct(char *head, bChunkInd& dataChunk) if (mFlags & FD_ENDIAN_SWAP) swap(head, dataChunk, ignoreEndianFlag); - + if (!mFileDNA->flagEqual(dataChunk.dna_nr)) { @@ -656,7 +657,7 @@ char* bFile::readStruct(char *head, bChunkInd& dataChunk) oldStruct = mFileDNA->getStruct(dataChunk.dna_nr); oldType = mFileDNA->getType(oldStruct[0]); - + oldLen = mFileDNA->getLength(oldStruct[0]); if ((mFlags&FD_BROKEN_DNA)!=0) @@ -951,7 +952,7 @@ void bFile::safeSwapPtr(char *dst, const char *src) B3_SWITCH_LONGINT(longValue); *((int*)dst) = (int)(longValue>>3); } - + } else if (ptrMem==8 && ptrFile==4) { @@ -991,7 +992,7 @@ void bFile::getMatchingFileDNA(short* dna_addr, const char* lookupName, const c const char* type = mFileDNA->getType(dna_addr[0]); const char* name = mFileDNA->getName(dna_addr[1]); - + int eleLen = mFileDNA->getElementSize(dna_addr[0], dna_addr[1]); @@ -1008,7 +1009,7 @@ void bFile::getMatchingFileDNA(short* dna_addr, const char* lookupName, const c //int arrayLenold = mFileDNA->getArraySize((char*)name.c_str()); int arrayLen = mFileDNA->getArraySizeNew(dna_addr[1]); //assert(arrayLenold == arrayLen); - + if (name[0] == '*') { // cast pointers @@ -1022,11 +1023,11 @@ void bFile::getMatchingFileDNA(short* dna_addr, const char* lookupName, const c { //void **sarray = (void**)strcData; //void **darray = (void**)data; - + char *cpc, *cpo; cpc = (char*)strcData; cpo = (char*)data; - + for (int a=0; agetStruct(dna_nr); - + int elementLength = oldStruct[1]; oldStruct+=2; @@ -1249,8 +1250,8 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose memType = fileDna->getType(oldStruct[0]); memName = fileDna->getName(oldStruct[1]); - - + + int arrayLen = fileDna->getArraySizeNew(oldStruct[1]); if (memName[0] == '*') @@ -1268,7 +1269,7 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose } //skip the * printf("<%s type=\"pointer\"> ",&memName[1]); - printf("%d ", array[a]); + printf("%p ", array[a]); printf("\n",&memName[1]); } @@ -1286,7 +1287,7 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose printf(" "); } printf("<%s type=\"pointer\"> ",&memName[1]); - printf("%d ", ptr); + printf("%p ", ptr); printf("\n",&memName[1]); } ptr = findLibPointer(ptr); @@ -1316,7 +1317,7 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose } else { int revType = fileDna->getReverseType(oldStruct[0]); - if (oldStruct[0]>=firstStructType) //revType != -1 && + if (oldStruct[0]>=firstStructType) //revType != -1 && { char cleanName[MAX_STRLEN]; getCleanName(memName,cleanName); @@ -1421,7 +1422,7 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose } } } - + } } } @@ -1429,7 +1430,7 @@ int bFile::resolvePointersStructRecursive(char *strcPtr, int dna_nr, int verbose int size = fileDna->getElementSize(oldStruct[0], oldStruct[1]); totalSize += size; elemPtr+=size; - + } return totalSize; @@ -1445,9 +1446,9 @@ void bFile::resolvePointers(int verboseMode) if (1) //mFlags & (FD_BITS_VARIES | FD_VERSION_VARIES)) { - resolvePointersMismatch(); + resolvePointersMismatch(); } - + { if (verboseMode & FD_VERBOSE_EXPORT_XML) @@ -1465,9 +1466,9 @@ void bFile::resolvePointers(int verboseMode) //dataChunk.len short int* oldStruct = fileDna->getStruct(dataChunk.dna_nr); char* oldType = fileDna->getType(oldStruct[0]); - + if (verboseMode & FD_VERBOSE_EXPORT_XML) - printf(" <%s pointer=%d>\n",oldType,dataChunk.oldPtr); + printf(" <%s pointer=%p>\n",oldType,dataChunk.oldPtr); resolvePointersChunk(dataChunk, verboseMode); @@ -1483,8 +1484,8 @@ void bFile::resolvePointers(int verboseMode) printf("\n"); } } - - + + } @@ -1518,13 +1519,13 @@ void bFile::dumpChunks(bParse::bDNA* dna) bChunkInd& dataChunk = m_chunks[i]; char* codeptr = (char*)&dataChunk.code; char codestr[5] = {codeptr[0],codeptr[1],codeptr[2],codeptr[3],0}; - + short* newStruct = dna->getStruct(dataChunk.dna_nr); char* typeName = dna->getType(newStruct[0]); printf("%3d: %s ",i,typeName); printf("code=%s ",codestr); - + printf("ptr=%p ",dataChunk.oldPtr); printf("len=%d ",dataChunk.len); printf("nr=%d ",dataChunk.nr); @@ -1534,8 +1535,8 @@ void bFile::dumpChunks(bParse::bDNA* dna) } printf("\n"); - - + + } @@ -1544,7 +1545,7 @@ void bFile::dumpChunks(bParse::bDNA* dna) ifd.success = 0; ifd.IDname = NULL; ifd.just_print_it = 1; - for (i=0; im_blocks.size(); ++i) + for (i=0; im_blocks.size(); ++i) { BlendBlock* bb = bf->m_blocks[i]; printf("tag='%s'\tptr=%p\ttype=%s\t[%4d]", bb->tag, bb,bf->types[bb->type_index].name,bb->m_array_entries_.size()); @@ -1563,7 +1564,7 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers) for (int i=0;igetLength(oldStruct[0]); ///don't try to convert Link block data, just memcpy it. Other data can be converted. reverseOld = mMemoryDNA->getReverseType(oldType); - + if ((reverseOld!=-1)) { @@ -1586,7 +1587,7 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers) // make sure it's the same assert((strcmp(oldType, newType)==0) && "internal error, struct mismatch!"); - + curLen = mMemoryDNA->getLength(curStruct[0]); dataChunk.dna_nr = reverseOld; if (strcmp("Link",oldType)!=0) @@ -1596,10 +1597,10 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers) { // printf("keep length of link = %d\n",dataChunk.len); } - + //write the structure header fwrite(&dataChunk,sizeof(bChunkInd),1,fp); - + short int* curStruct1; @@ -1615,7 +1616,7 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers) printf("serious error, struct mismatch: don't write\n"); } } - + } @@ -1625,9 +1626,9 @@ int bFile::getNextBlock(bChunkInd *dataChunk, const char *dataPtr, const int fl bool swap = false; bool varies = false; - if (flags &FD_ENDIAN_SWAP) + if (flags &FD_ENDIAN_SWAP) swap = true; - if (flags &FD_BITS_VARIES) + if (flags &FD_BITS_VARIES) varies = true; if (VOID_IS_8) @@ -1697,11 +1698,11 @@ int bFile::getNextBlock(bChunkInd *dataChunk, const char *dataPtr, const int fl { b3Long64 oldPtr =0; memcpy(&oldPtr, &head.m_uniqueInts[0], 8); - if (swap) + if (swap) B3_SWITCH_LONGINT(oldPtr); chunk.m_uniqueInt = (int)(oldPtr >> 3); } - + chunk.dna_nr = head.dna_nr; chunk.nr = head.nr; diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 95443af50..2ca20cdd8 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -38,8 +38,9 @@ static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, const btDbvtVolume& b) { #if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); - btDbvtVolume& res=*(btDbvtVolume*)locals; + ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume* ptr = (btDbvtVolume*) locals; + btDbvtVolume& res=*ptr; #else btDbvtVolume res; #endif @@ -250,7 +251,8 @@ static btDbvtVolume bounds( const tNodeArray& leaves) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); - btDbvtVolume& volume=*(btDbvtVolume*)locals; + btDbvtVolume* ptr = (btDbvtVolume*) locals; + btDbvtVolume& volume=*ptr; volume=leaves[0]->volume; #else btDbvtVolume volume=leaves[0]->volume; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 6f88c6d18..395df3a55 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -31,11 +31,11 @@ btCollisionObject::btCollisionObject() m_activationState1(1), m_deactivationTime(btScalar(0.)), m_friction(btScalar(0.5)), - m_rollingFriction(0.0f), m_restitution(btScalar(0.)), + m_rollingFriction(0.0f), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), - m_userIndex(-1), + m_userIndex(-1), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), m_ccdMotionThreshold(btScalar(0.)), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h index 4daa9441d..952440b7d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -1,43 +1,43 @@ -#ifndef BT_COLLISION_OBJECT_WRAPPER_H -#define BT_COLLISION_OBJECT_WRAPPER_H - -///btCollisionObjectWrapperis an internal data structure. -///Most users can ignore this and use btCollisionObject and btCollisionShape instead -class btCollisionShape; -class btCollisionObject; -class btTransform; -#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition - -#define BT_DECLARE_STACK_ONLY_OBJECT \ - private: \ - void* operator new(size_t size); \ - void operator delete(void*); - -struct btCollisionObjectWrapper; -struct btCollisionObjectWrapper -{ -BT_DECLARE_STACK_ONLY_OBJECT - -private: - btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. - btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); - -public: - const btCollisionObjectWrapper* m_parent; - const btCollisionShape* m_shape; - const btCollisionObject* m_collisionObject; - const btTransform& m_worldTransform; - int m_partId; - int m_index; - - btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), - m_partId(partId), m_index(index) - {} - - SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } - SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } -}; - -#endif //BT_COLLISION_OBJECT_WRAPPER_H +#ifndef BT_COLLISION_OBJECT_WRAPPER_H +#define BT_COLLISION_OBJECT_WRAPPER_H + +///btCollisionObjectWrapperis an internal data structure. +///Most users can ignore this and use btCollisionObject and btCollisionShape instead +class btCollisionShape; +class btCollisionObject; +class btTransform; +#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition + +#define BT_DECLARE_STACK_ONLY_OBJECT \ + private: \ + void* operator new(size_t size); \ + void operator delete(void*); + +struct btCollisionObjectWrapper; +struct btCollisionObjectWrapper +{ +BT_DECLARE_STACK_ONLY_OBJECT + +private: + btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. + btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); + +public: + const btCollisionObjectWrapper* m_parent; + const btCollisionShape* m_shape; + const btCollisionObject* m_collisionObject; + const btTransform& m_worldTransform; + int m_partId; + int m_index; + + btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), + m_partId(partId), m_index(index) + {} + + SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } + SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } +}; + +#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index bdd60a541..1960f521d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -292,7 +292,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); - bool condition = true; + btConvexCast* convexCasterPtr = 0; //use kF_UseSubSimplexConvexCastRaytest by default if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest) @@ -1245,7 +1245,10 @@ public: void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { // Draw a small simplex at the center of the object - getDebugDrawer()->drawTransform(worldTransform,1); + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(worldTransform,1); + } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index af8cede74..a80c438d1 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -123,7 +123,7 @@ public: //backup btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - btTransform orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(index); btTransform newChildWorldTrans = orgTrans*childTrans ; @@ -294,7 +294,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap btManifoldArray manifoldArray; const btCollisionShape* childShape = 0; btTransform orgTrans; - btTransform orgInterpolationTrans; + btTransform newChildWorldTrans; btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; @@ -304,8 +304,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { childShape = compoundShape->getChildShape(i); //if not longer overlapping, remove the algorithm - orgTrans = colObjWrap->getWorldTransform(); - orgInterpolationTrans = colObjWrap->getWorldTransform(); + orgTrans = colObjWrap->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); newChildWorldTrans = orgTrans*childTrans ; diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index b0103fc8c..1d64d84b8 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -1,427 +1,426 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -#include "btCompoundCompoundCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btAabbUtil2.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - - -btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; - -btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) -{ - - void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); - m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); - - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - - const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - - const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); - m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); - - const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); - m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - - -} - - -btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() -{ - removeChildAlgorithms(); - m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); - btAlignedFree(m_childCollisionAlgorithmCache); -} - -void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) -{ - int i; - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); - } - } -} - - -void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() -{ - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - - int numChildren = pairs.size(); - int i; - for (i=0;i~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algo); - } - } - m_childCollisionAlgorithmCache->removeAllPairs(); -} - -struct btCompoundCompoundLeafCallback : btDbvt::ICollide -{ - int m_numOverlapPairs; - - - const btCollisionObjectWrapper* m_compound0ColObjWrap; - const btCollisionObjectWrapper* m_compound1ColObjWrap; - btDispatcher* m_dispatcher; - const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - - btPersistentManifold* m_sharedManifold; - - btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, - const btCollisionObjectWrapper* compound0ObjWrap, - btDispatcher* dispatcher, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut, - btHashedSimplePairCache* childAlgorithmsCache, - btPersistentManifold* sharedManifold) - :m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithmCache(childAlgorithmsCache), - m_sharedManifold(sharedManifold), - m_numOverlapPairs(0) - { - - } - - - - - void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) - { - m_numOverlapPairs++; - - - int childIndex0 = leaf0->dataAsInt; - int childIndex1 = leaf1->dataAsInt; - - - btAssert(childIndex0>=0); - btAssert(childIndex1>=0); - - - const btCompoundShape* compoundShape0 = static_cast(m_compound0ColObjWrap->getCollisionShape()); - btAssert(childIndex0getNumChildShapes()); - - const btCompoundShape* compoundShape1 = static_cast(m_compound1ColObjWrap->getCollisionShape()); - btAssert(childIndex1getNumChildShapes()); - - const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); - const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); - - //backup - btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); - const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); - btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; - - btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); - const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); - btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; - - - //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - - if (gCompoundCompoundChildShapePairCallback) - { - if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) - return; - } - - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { - btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); - btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); - - - btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); - - btCollisionAlgorithm* colAlgo = 0; - - if (pair) - { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else - { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; - } - - btAssert(colAlgo); - - const btCollisionObjectWrapper* tmpWrap0 = 0; - const btCollisionObjectWrapper* tmpWrap1 = 0; - - tmpWrap0 = m_resultOut->getBody0Wrap(); - tmpWrap1 = m_resultOut->getBody1Wrap(); - - m_resultOut->setBody0Wrap(&compoundWrap0); - m_resultOut->setBody1Wrap(&compoundWrap1); - - m_resultOut->setShapeIdentifiersA(-1,childIndex0); - m_resultOut->setShapeIdentifiersB(-1,childIndex1); - - - colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); - - m_resultOut->setBody0Wrap(tmpWrap0); - m_resultOut->setBody1Wrap(tmpWrap1); - - - - } - } -}; - - -static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform) -{ - btVector3 newmin,newmax; - btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); - btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); - return Intersect(a,newb); -} - - -static inline void MycollideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - btCompoundCompoundLeafCallback* callback) -{ - - if(root0&&root1) - { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); - stkStack[0]=btDbvt::sStkNN(root0,root1); - do { - btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(p.a->volume,p.b->volume,xform)) - { - if(depth>treshold) - { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; - } - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); - } - } - else - { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); - } - else - { - callback->Process(p.a,p.b); - } - } - } - } while(depth); - } -} - -void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; - - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); - const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); - - const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); - const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); - if (!tree0 || !tree1) - { - return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); - } - ///btCompoundShape might have changed: - ////make sure the internal child collision algorithm caches are still valid - if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) - { - ///clear all - removeChildAlgorithms(); - m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); - m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - - } - - - ///we need to refresh all contact manifolds - ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep - ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm - { - int i; - btManifoldArray manifoldArray; - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); - for (int m=0;mgetNumContacts()) - { - resultOut->setPersistentManifold(manifoldArray[m]); - resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0); - } - } - manifoldArray.resize(0); - } - } - } - - - - - btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); - - - const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); - MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); - - //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); - - //remove non-overlapping child pairs - - { - btAssert(m_removePairs.size()==0); - - //iterate over all children, perform an AABB check inside ProcessChildShape - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - - int i; - btManifoldArray manifoldArray; - - - - - - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;igetChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); - const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - } - - { - btTransform orgInterpolationTrans1; - const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; - btTransform newChildWorldTrans1; - - childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); - const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - } - - - - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { - algo->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algo); - m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); - } - } - } - for (int i=0;iremoveOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); - } - m_removePairs.clear(); - } - -} - -btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - btAssert(0); - return 0.f; - -} - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#include "btCompoundCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + + +btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; + +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) +{ + + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); + m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + + const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + +} + + +btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); + m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); + btAlignedFree(m_childCollisionAlgorithmCache); +} + +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + int i; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;igetAllContactManifolds(manifoldArray); + } + } +} + + +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +{ + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int numChildren = pairs.size(); + int i; + for (i=0;i~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } + } + m_childCollisionAlgorithmCache->removeAllPairs(); +} + +struct btCompoundCompoundLeafCallback : btDbvt::ICollide +{ + int m_numOverlapPairs; + + + const btCollisionObjectWrapper* m_compound0ColObjWrap; + const btCollisionObjectWrapper* m_compound1ColObjWrap; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + + btPersistentManifold* m_sharedManifold; + + btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithmCache(childAlgorithmsCache), + m_sharedManifold(sharedManifold) + { + + } + + + + + void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + { + m_numOverlapPairs++; + + + int childIndex0 = leaf0->dataAsInt; + int childIndex1 = leaf1->dataAsInt; + + + btAssert(childIndex0>=0); + btAssert(childIndex1>=0); + + + const btCompoundShape* compoundShape0 = static_cast(m_compound0ColObjWrap->getCollisionShape()); + btAssert(childIndex0getNumChildShapes()); + + const btCompoundShape* compoundShape1 = static_cast(m_compound1ColObjWrap->getCollisionShape()); + btAssert(childIndex1getNumChildShapes()); + + const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); + const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); + + //backup + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); + btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); + btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; + + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + + if (gCompoundCompoundChildShapePairCallback) + { + if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + return; + } + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); + + + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + + btCollisionAlgorithm* colAlgo = 0; + + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + + } else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } + + btAssert(colAlgo); + + const btCollisionObjectWrapper* tmpWrap0 = 0; + const btCollisionObjectWrapper* tmpWrap1 = 0; + + tmpWrap0 = m_resultOut->getBody0Wrap(); + tmpWrap1 = m_resultOut->getBody1Wrap(); + + m_resultOut->setBody0Wrap(&compoundWrap0); + m_resultOut->setBody1Wrap(&compoundWrap1); + + m_resultOut->setShapeIdentifiersA(-1,childIndex0); + m_resultOut->setShapeIdentifiersB(-1,childIndex1); + + + colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); + + m_resultOut->setBody0Wrap(tmpWrap0); + m_resultOut->setBody1Wrap(tmpWrap1); + + + + } + } +}; + + +static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform) +{ + btVector3 newmin,newmax; + btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); + return Intersect(a,newb); +} + + +static inline void MycollideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback) +{ + + if(root0&&root1) + { + int depth=1; + int treshold=btDbvt::DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); + stkStack[0]=btDbvt::sStkNN(root0,root1); + do { + btDbvt::sStkNN p=stkStack[--depth]; + if(MyIntersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); + } + else + { + callback->Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); + const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); + + const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); + const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + if (!tree0 || !tree1) + { + return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + } + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) + { + ///clear all + removeChildAlgorithms(); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + } + + + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + btManifoldArray manifoldArray; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;igetAllContactManifolds(manifoldArray); + for (int m=0;mgetNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0); + } + } + manifoldArray.resize(0); + } + } + } + + + + + btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); + + + const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + + //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); + + //remove non-overlapping child pairs + + { + btAssert(m_removePairs.size()==0); + + //iterate over all children, perform an AABB check inside ProcessChildShape + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int i; + btManifoldArray manifoldArray; + + + + + + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + + for (i=0;igetChildShape(pairs[i].m_indexA); + orgTrans0 = col0ObjWrap->getWorldTransform(); + orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); + newChildWorldTrans0 = orgTrans0*childTrans0 ; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + } + + { + btTransform orgInterpolationTrans1; + const btCollisionShape* childShape1 = 0; + btTransform orgTrans1; + btTransform newChildWorldTrans1; + + childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); + orgTrans1 = col1ObjWrap->getWorldTransform(); + orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); + newChildWorldTrans1 = orgTrans1*childTrans1 ; + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + } + + + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + } + } + } + for (int i=0;iremoveOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + } + m_removePairs.clear(); + } + +} + +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + return 0.f; + +} + + + diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 5fe3fc443..06a762f20 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -1,89 +1,89 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H -#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H - -#include "btCompoundCollisionAlgorithm.h" - -#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" -class btDispatcher; -class btCollisionObject; - -class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); -extern btShapePairCallback gCompoundCompoundChildShapePairCallback; - -/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm -{ - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - btSimplePairArray m_removePairs; - - - int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision1; - - void removeChildAlgorithms(); - -// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - -public: - - btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); - - virtual ~btCompoundCompoundCollisionAlgorithm(); - - - - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); - } - }; - -}; - -#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H + +#include "btCompoundCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" +class btDispatcher; +class btCollisionObject; + +class btCollisionShape; +typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); +extern btShapePairCallback gCompoundCompoundChildShapePairCallback; + +/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm +{ + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btSimplePairArray m_removePairs; + + + int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; + + void removeChildAlgorithms(); + +// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + + btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btCompoundCompoundCollisionAlgorithm(); + + + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index e23f5f7a8..912a52855 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -88,20 +88,19 @@ partId, int triangleIndex) //just for debugging purposes //printf("triangle %d",m_triangleCount++); - const btCollisionObject* ob = const_cast(m_triBodyWrap->getCollisionObject()); + btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - //const btCollisionObject* ob = static_cast(m_triBodyWrap->getCollisionObject()); - - #if 0 + ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { + const btCollisionObject* ob = const_cast(m_triBodyWrap->getCollisionObject()); btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index c3cacec4a..dac2711e4 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -105,8 +105,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); - int sl = sizeof(btConvexSeparatingDistanceUtil); - sl = sizeof(btGjkPairDetector); + int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 933400bb1..cfcca5654 100644 --- a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -1,278 +1,278 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btHashedSimplePairCache.h" - - -#include - -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; - - - - -btHashedSimplePairCache::btHashedSimplePairCache(): - m_blockedForChanges(false) -{ - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); - growTables(); -} - - - - -btHashedSimplePairCache::~btHashedSimplePairCache() -{ -} - - - - - - -void btHashedSimplePairCache::removeAllPairs() -{ - m_overlappingPairArray.clear(); - m_hashTable.clear(); - m_next.clear(); - - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); - growTables(); -} - - - -btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) -{ - gFindSimplePairs++; - - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - - if (hash >= m_hashTable.size()) - { - return NULL; - } - - int index = m_hashTable[hash]; - while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) - { - index = m_next[index]; - } - - if (index == BT_SIMPLE_NULL_PAIR) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; -} - -//#include - -void btHashedSimplePairCache::growTables() -{ - - int newCapacity = m_overlappingPairArray.capacity(); - - if (m_hashTable.size() < newCapacity) - { - //grow hashtable and next table - int curHashtableSize = m_hashTable.size(); - - m_hashTable.resize(newCapacity); - m_next.resize(newCapacity); - - - int i; - - for (i= 0; i < newCapacity; ++i) - { - m_hashTable[i] = BT_SIMPLE_NULL_PAIR; - } - for (i = 0; i < newCapacity; ++i) - { - m_next[i] = BT_SIMPLE_NULL_PAIR; - } - - for(i=0;i(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - m_next[i] = m_hashTable[hashValue]; - m_hashTable[hashValue] = i; - } - - - } -} - -btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) -{ - - int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - - - btSimplePair* pair = internalFindPair(indexA, indexB, hash); - if (pair != NULL) - { - return pair; - } - - int count = m_overlappingPairArray.size(); - int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expandNonInitializing(); - - int newCapacity = m_overlappingPairArray.capacity(); - - if (oldCapacity < newCapacity) - { - growTables(); - //hash with new capacity - hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - } - - pair = new (mem) btSimplePair(indexA,indexB); - - pair->m_userPointer = 0; - - m_next[count] = m_hashTable[hash]; - m_hashTable[hash] = count; - - return pair; -} - - - -void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) -{ - gRemoveSimplePairs++; - - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - - btSimplePair* pair = internalFindPair(indexA, indexB, hash); - if (pair == NULL) - { - return 0; - } - - - void* userData = pair->m_userPointer; - - - int pairIndex = int(pair - &m_overlappingPairArray[0]); - btAssert(pairIndex < m_overlappingPairArray.size()); - - // Remove the pair from the hash table. - int index = m_hashTable[hash]; - btAssert(index != BT_SIMPLE_NULL_PAIR); - - int previous = BT_SIMPLE_NULL_PAIR; - while (index != pairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_SIMPLE_NULL_PAIR) - { - btAssert(m_next[previous] == pairIndex); - m_next[previous] = m_next[pairIndex]; - } - else - { - m_hashTable[hash] = m_next[pairIndex]; - } - - // We now move the last pair into spot of the - // pair being removed. We need to fix the hash - // table indices to support the move. - - int lastPairIndex = m_overlappingPairArray.size() - 1; - - // If the removed pair is the last pair, we are done. - if (lastPairIndex == pairIndex) - { - m_overlappingPairArray.pop_back(); - return userData; - } - - // Remove the last pair from the hash table. - const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast(getHash(static_cast(last->m_indexA), static_cast(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); - - index = m_hashTable[lastHash]; - btAssert(index != BT_SIMPLE_NULL_PAIR); - - previous = BT_SIMPLE_NULL_PAIR; - while (index != lastPairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_SIMPLE_NULL_PAIR) - { - btAssert(m_next[previous] == lastPairIndex); - m_next[previous] = m_next[lastPairIndex]; - } - else - { - m_hashTable[lastHash] = m_next[lastPairIndex]; - } - - // Copy the last pair into the remove pair's spot. - m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; - - // Insert the last pair into the hash table - m_next[pairIndex] = m_hashTable[lastHash]; - m_hashTable[lastHash] = pairIndex; - - m_overlappingPairArray.pop_back(); - - return userData; -} -//#include - - - - - - - - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btHashedSimplePairCache.h" + + +#include + +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs =0; +int gAddedSimplePairs =0; +int gFindSimplePairs =0; + + + + +btHashedSimplePairCache::btHashedSimplePairCache(): + m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedSimplePairCache::~btHashedSimplePairCache() +{ +} + + + + + + +void btHashedSimplePairCache::removeAllPairs() +{ + m_overlappingPairArray.clear(); + m_hashTable.clear(); + m_next.clear(); + + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + +btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) +{ + gFindSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) + { + index = m_next[index]; + } + + if (index == BT_SIMPLE_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include + +void btHashedSimplePairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_SIMPLE_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_SIMPLE_NULL_PAIR; + } + + for(i=0;i(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) +{ + + int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair != NULL) + { + return pair; + } + + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btSimplePair(indexA,indexB); + + pair->m_userPointer = 0; + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) +{ + gRemoveSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair == NULL) + { + return 0; + } + + + void* userData = pair->m_userPointer; + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + int previous = BT_SIMPLE_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->m_indexA), static_cast(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + previous = BT_SIMPLE_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include + + + + + + + + + + diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index ba573743d..e88ef97e9 100644 --- a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -1,174 +1,174 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H -#define BT_HASHED_SIMPLE_PAIR_CACHE_H - - - -#include "LinearMath/btAlignedObjectArray.h" - -const int BT_SIMPLE_NULL_PAIR=0xffffffff; - -struct btSimplePair -{ - btSimplePair(int indexA,int indexB) - :m_indexA(indexA), - m_indexB(indexB), - m_userPointer(0) - { - } - - int m_indexA; - int m_indexB; - union - { - void* m_userPointer; - int m_userValue; - }; -}; - -typedef btAlignedObjectArray btSimplePairArray; - - - -extern int gOverlappingSimplePairs; -extern int gRemoveSimplePairs; -extern int gAddedSimplePairs; -extern int gFindSimplePairs; - - - - -class btHashedSimplePairCache -{ - btSimplePairArray m_overlappingPairArray; - - bool m_blockedForChanges; - - -protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - - -public: - btHashedSimplePairCache(); - virtual ~btHashedSimplePairCache(); - - void removeAllPairs(); - - virtual void* removeOverlappingPair(int indexA,int indexB); - - // Add a pair and return the new pair. If the pair already exists, - // no new pair is created and the old one is returned. - virtual btSimplePair* addOverlappingPair(int indexA,int indexB) - { - gAddedSimplePairs++; - - return internalAddPair(indexA,indexB); - } - - - virtual btSimplePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btSimplePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - btSimplePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btSimplePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - - btSimplePair* findPair(int indexA,int indexB); - - int GetCount() const { return m_overlappingPairArray.size(); } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } -private: - - btSimplePair* internalAddPair(int indexA, int indexB); - - void growTables(); - - SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) - { - return pair.m_indexA == indexA && pair.m_indexB == indexB; - } - - - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) - { - int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); - // Thomas Wang's hash - - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); - return static_cast(key); - } - - - - - - SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) - { - - int index = m_hashTable[hash]; - - while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) - { - index = m_next[index]; - } - - if ( index == BT_SIMPLE_NULL_PAIR ) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; - } - - -}; - - - - -#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H +#define BT_HASHED_SIMPLE_PAIR_CACHE_H + + + +#include "LinearMath/btAlignedObjectArray.h" + +const int BT_SIMPLE_NULL_PAIR=0xffffffff; + +struct btSimplePair +{ + btSimplePair(int indexA,int indexB) + :m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) + { + } + + int m_indexA; + int m_indexB; + union + { + void* m_userPointer; + int m_userValue; + }; +}; + +typedef btAlignedObjectArray btSimplePairArray; + + + +extern int gOverlappingSimplePairs; +extern int gRemoveSimplePairs; +extern int gAddedSimplePairs; +extern int gFindSimplePairs; + + + + +class btHashedSimplePairCache +{ + btSimplePairArray m_overlappingPairArray; + + bool m_blockedForChanges; + + +protected: + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + + +public: + btHashedSimplePairCache(); + virtual ~btHashedSimplePairCache(); + + void removeAllPairs(); + + virtual void* removeOverlappingPair(int indexA,int indexB); + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + { + gAddedSimplePairs++; + + return internalAddPair(indexA,indexB); + } + + + virtual btSimplePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btSimplePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btSimplePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btSimplePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + btSimplePair* findPair(int indexA,int indexB); + + int GetCount() const { return m_overlappingPairArray.size(); } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btSimplePair* internalAddPair(int indexA, int indexB); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) + { + return pair.m_indexA == indexA && pair.m_indexB == indexB; + } + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + { + int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast(key); + } + + + + + + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + { + + int index = m_hashTable[hash]; + + while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + { + index = m_next[index]; + } + + if ( index == BT_SIMPLE_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + +}; + + + + +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H + + diff --git a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index f97d11db7..73fa4e87e 100644 --- a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -114,7 +114,6 @@ struct btConnectivityProcessor : public btTriangleCallback if(numshared >= 3) return; } - switch (numshared) { case 0: @@ -203,6 +202,7 @@ struct btConnectivityProcessor : public btTriangleCallback ang4 = 0.f; } else { + calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); @@ -213,7 +213,7 @@ struct btConnectivityProcessor : public btTriangleCallback isConvex = (dotA<0.); correctedAngle = isConvex ? ang4 : -ang4; - btQuaternion orn2 = btQuaternion(btVector3(calculatedEdge.x(), calculatedEdge.y(), calculatedEdge.z()),-correctedAngle); + btQuaternion orn2(calculatedEdge,-correctedAngle); calculatedNormalB = btMatrix3x3(orn2)*normalA; @@ -301,6 +301,11 @@ struct btConnectivityProcessor : public btTriangleCallback break; } + default: + { + // printf("warning: duplicate triangle\n"); + } + } } }; diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 7578bb258..f8c55ace4 100644 --- a/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -117,6 +117,7 @@ public: ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData* dataBuffer); }; @@ -181,4 +182,13 @@ SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSeri return "btCapsuleShapeData"; } +SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) +{ + m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions); + m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin; + m_localScaling.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_localScaling); + //it is best to already pre-allocate the matching btCapsuleShape*(X/Z) version to match m_upAxis + m_upAxis = dataBuffer->m_upAxis; +} + #endif //BT_CAPSULE_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btCollisionShape.h b/src/BulletCollision/CollisionShapes/btCollisionShape.h index 1d8a4dc30..5e8656800 100644 --- a/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -28,18 +28,14 @@ ATTRIBUTE_ALIGNED16(class) btCollisionShape { protected: int m_shapeType; + void* m_userPointer; + int m_userIndex; - union - { - void* m_userPointer; - int m_userIndex; - }; - public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0) + btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) { } @@ -135,17 +131,17 @@ public: { return m_userPointer; } - void setUserIndex(int index) { m_userIndex = index; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 0aa75f2bf..8842fa2f1 100644 --- a/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -77,8 +77,8 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio if (m_dynamicAabbTree) { const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - int index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); + size_t index = m_children.size(); + child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast(index) ); } m_children.push_back(child); @@ -312,7 +312,8 @@ void btCompoundShape::createAabbTreeFromChildren() child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); + size_t index2 = index; + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast(index2) ); } } } diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index c31df91a7..f4324c1f4 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -274,7 +274,7 @@ void btConvexPolyhedron::initialize() #endif } -void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const { minProj = FLT_MAX; maxProj = -FLT_MAX; diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h index 75679eb3b..d3cd066ac 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron void initialize(); bool testContainment() const; - void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; }; diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index bf255dfc8..6cae7c550 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -365,13 +365,13 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback { //first triangle getVertex(x,j,vertices[0]); - getVertex(x, j + 1, vertices[1]); + getVertex(x, j + 1, vertices[1]); getVertex(x + 1, j + 1, vertices[2]); callback->processTriangle(vertices,x,j); //second triangle // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman getVertex(x+1,j+1,vertices[1]); - getVertex(x + 1, j, vertices[2]); + getVertex(x + 1, j, vertices[2]); callback->processTriangle(vertices, x, j); } else diff --git a/src/BulletCollision/Gimpact/btCompoundFromGimpact.h b/src/BulletCollision/Gimpact/btCompoundFromGimpact.h index 9e66285c1..02f8b678a 100644 --- a/src/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ b/src/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -1,93 +1,93 @@ -#ifndef BT_COMPOUND_FROM_GIMPACT -#define BT_COMPOUND_FROM_GIMPACT - -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "btGImpactShape.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" - -struct MyCallback : public btTriangleRaycastCallback - { - int m_ignorePart; - int m_ignoreTriangleIndex; - - - MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) - :btTriangleRaycastCallback(from,to), - m_ignorePart(ignorePart), - m_ignoreTriangleIndex(ignoreTriangleIndex) - { - - } - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) - { - if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex) - { - if (hitFraction < m_hitFraction) - return hitFraction; - } - - return m_hitFraction; - } - }; - struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback - { - const btGImpactMeshShape* m_gimpactShape; - btCompoundShape* m_colShape; - btScalar m_depth; - - MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) - :m_colShape(colShape), - m_gimpactShape(meshShape), - m_depth(depth) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - btVector3 scale = m_gimpactShape->getLocalScaling(); - btVector3 v0=triangle[0]*scale; - btVector3 v1=triangle[1]*scale; - btVector3 v2=triangle[2]*scale; - - btVector3 centroid = (v0+v1+v2)/3; - btVector3 normal = (v1-v0).cross(v2-v0); - normal.normalize(); - btVector3 rayFrom = centroid; - btVector3 rayTo = centroid-normal*m_depth; - - MyCallback cb(rayFrom,rayTo,partId,triangleIndex); - - m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo); - if (cb.m_hitFraction<1) - { - rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction); - //rayTo = cb.m_from; - //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); - //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); - } - - - - btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo); - btTransform ident; - ident.setIdentity(); - m_colShape->addChildShape(ident,tet); - } - }; - -btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) -{ - btCompoundShape* colShape = new btCompoundShape(); - - btTransform tr; - tr.setIdentity(); - - MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth); - btVector3 aabbMin,aabbMax; - gimpactMesh->getAabb(tr,aabbMin,aabbMax); - gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax); - - return colShape; -} - +#ifndef BT_COMPOUND_FROM_GIMPACT +#define BT_COMPOUND_FROM_GIMPACT + +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "btGImpactShape.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" + +struct MyCallback : public btTriangleRaycastCallback + { + int m_ignorePart; + int m_ignoreTriangleIndex; + + + MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) + :btTriangleRaycastCallback(from,to), + m_ignorePart(ignorePart), + m_ignoreTriangleIndex(ignoreTriangleIndex) + { + + } + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex) + { + if (hitFraction < m_hitFraction) + return hitFraction; + } + + return m_hitFraction; + } + }; + struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback + { + const btGImpactMeshShape* m_gimpactShape; + btCompoundShape* m_colShape; + btScalar m_depth; + + MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) + :m_colShape(colShape), + m_gimpactShape(meshShape), + m_depth(depth) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + btVector3 scale = m_gimpactShape->getLocalScaling(); + btVector3 v0=triangle[0]*scale; + btVector3 v1=triangle[1]*scale; + btVector3 v2=triangle[2]*scale; + + btVector3 centroid = (v0+v1+v2)/3; + btVector3 normal = (v1-v0).cross(v2-v0); + normal.normalize(); + btVector3 rayFrom = centroid; + btVector3 rayTo = centroid-normal*m_depth; + + MyCallback cb(rayFrom,rayTo,partId,triangleIndex); + + m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo); + if (cb.m_hitFraction<1) + { + rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction); + //rayTo = cb.m_from; + //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); + //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); + } + + + + btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo); + btTransform ident; + ident.setIdentity(); + m_colShape->addChildShape(ident,tet); + } + }; + +btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) +{ + btCompoundShape* colShape = new btCompoundShape(); + + btTransform tr; + tr.setIdentity(); + + MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth); + btVector3 aabbMin,aabbMax; + gimpactMesh->getAabb(tr,aabbMin,aabbMax); + gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax); + + return colShape; +} + #endif //BT_COMPOUND_FROM_GIMPACT \ No newline at end of file diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index 2be45550e..ea131f0c4 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -178,62 +178,62 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans } #endif //TEST_INTERNAL_OBJECTS - - - SIMD_FORCE_INLINE void btSegmentsClosestPoints( - btVector3& ptsVector, - btVector3& offsetA, - btVector3& offsetB, - btScalar& tA, btScalar& tB, - const btVector3& translation, - const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) -{ - // compute the parameters of the closest points on each line segment - - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); - - btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - - if ( denom == 0.0f ) { - tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - tB = tA * dirA_dot_dirB - dirB_dot_trans; - - if ( tB < -hlenB ) { - tB = -hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } else if ( tB > hlenB ) { - tB = hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - // compute the closest points relative to segment centers. - - offsetA = dirA * tA; - offsetB = dirB * tB; - - ptsVector = translation - offsetA + offsetB; -} + + + SIMD_FORCE_INLINE void btSegmentsClosestPoints( + btVector3& ptsVector, + btVector3& offsetA, + btVector3& offsetB, + btScalar& tA, btScalar& tB, + const btVector3& translation, + const btVector3& dirA, btScalar hlenA, + const btVector3& dirB, btScalar hlenB ) +{ + // compute the parameters of the closest points on each line segment + + btScalar dirA_dot_dirB = btDot(dirA,dirB); + btScalar dirA_dot_trans = btDot(dirA,translation); + btScalar dirB_dot_trans = btDot(dirB,translation); + + btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; + + if ( denom == 0.0f ) { + tA = 0.0f; + } else { + tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + tB = tA * dirA_dot_dirB - dirB_dot_trans; + + if ( tB < -hlenB ) { + tB = -hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } else if ( tB > hlenB ) { + tB = hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + // compute the closest points relative to segment centers. + + offsetA = dirA * tA; + offsetB = dirB * tB; + + ptsVector = translation - offsetA + offsetB; +} @@ -313,7 +313,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& int edgeB=-1; btVector3 worldEdgeA; btVector3 worldEdgeB; - btVector3 witnessPointA,witnessPointB; + btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0); int curEdgeEdge = 0; @@ -369,23 +369,23 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& // printf("edge-edge\n"); //add an edge-edge contact - btVector3 ptsVector; - btVector3 offsetA; - btVector3 offsetB; - btScalar tA; - btScalar tB; - - btVector3 translation = witnessPointB-witnessPointA; - - btVector3 dirA = worldEdgeA; - btVector3 dirB = worldEdgeB; - - btScalar hlenB = 1e30f; - btScalar hlenA = 1e30f; - - btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, - translation, - dirA, hlenA, + btVector3 ptsVector; + btVector3 offsetA; + btVector3 offsetB; + btScalar tA; + btScalar tB; + + btVector3 translation = witnessPointB-witnessPointA; + + btVector3 dirA = worldEdgeA; + btVector3 dirB = worldEdgeB; + + btScalar hlenB = 1e30f; + btScalar hlenA = 1e30f; + + btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, + translation, + dirA, hlenA, dirB,hlenB); btScalar nlSqrt = ptsVector.length2(); diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 4aecffd64..f2ed0cd39 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -21,7 +21,6 @@ subject to the following restrictions: struct btBroadphaseProxy; class btConvexShape; - class btTriangleRaycastCallback: public btTriangleCallback { public: @@ -33,7 +32,7 @@ public: //@BP Mod - allow backface filtering and unflipped normals enum EFlags { - kF_None = 0, + kF_None = 0, kF_FilterBackfaces = 1 << 0, kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. diff --git a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 18eb662de..ec638f60b 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -65,10 +65,10 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btVector3 n; n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - bool hasResult = false; + btVector3 c; - btScalar lastLambda = lambda; + btScalar dist2 = v.length2(); @@ -109,9 +109,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //m_simplexSolver->reset(); //check next line w = supVertexA-supVertexB; - lastLambda = lambda; + n = v; - hasResult = true; + } } ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. @@ -121,7 +121,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( if (m_simplexSolver->closest(v)) { dist2 = v.length2(); - hasResult = true; + //todo: check this normal for validity //n=v; //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); diff --git a/src/BulletCollision/premake4.lua b/src/BulletCollision/premake4.lua index 094b306ee..4e9fa1375 100644 --- a/src/BulletCollision/premake4.lua +++ b/src/BulletCollision/premake4.lua @@ -1,11 +1,21 @@ project "BulletCollision" - + kind "StaticLib" targetdir "../../lib" includedirs { "..", } files { - "**.cpp", - "**.h" - } \ No newline at end of file + "*.cpp", + "*.h", + "BroadphaseCollision/*.cpp", + "BroadphaseCollision/*.h", + "CollisionDispatch/*.cpp", + "CollisionDispatch/*.h", + "CollisionShapes/*.cpp", + "CollisionShapes/*.h", + "Gimpact/*.cpp", + "Gimpact/*.h", + "NarrowPhaseCollision/*.cpp", + "NarrowPhaseCollision/*.h", + } diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index eee0d6e28..e853c469e 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -381,8 +381,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co if (callback.hasHit()) { // we moved only a fraction - btScalar hitDistance; - hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); + //btScalar hitDistance; + //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); // m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 15a4c92de..6a0338631 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo() m_solveTwistLimit = false; m_solveSwingLimit = false; - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; + btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0); + btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0); b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); @@ -983,8 +983,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) { - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); + //btTransform trACur = m_rbA.getCenterOfMassTransform(); + //btTransform trBCur = m_rbB.getCenterOfMassTransform(); // btTransform trABCur = trBCur.inverse() * trACur; // btQuaternion qABCur = trABCur.getRotation(); // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9d60d9957..1098d0c96 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - btScalar a; - a=jacDiagABInv; + rel_vel = normal.dot(vel); diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index fd65bebe2..894059ad1 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -1,186 +1,186 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btFixedConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include - - -btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) -:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) -{ - m_frameInA = frameInA; - m_frameInB = frameInB; - -} - -btFixedConstraint::~btFixedConstraint () -{ -} - - -void btFixedConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 6; - info->nub = 0; -} - -void btFixedConstraint::getInfo2 (btConstraintInfo2* info) -{ - //fix the 3 linear degrees of freedom - - const btTransform& transA = m_rbA.getCenterOfMassTransform(); - const btTransform& transB = m_rbB.getCenterOfMassTransform(); - - const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); - const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); - - - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - } - - btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - // set right hand side for the linear dofs - btScalar k = info->fps * info->erp; - - btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = linearError[j]; - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - - btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - btVector3 swingAxis(0,0,0); - { - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - swingAxis = -ivB.cross(ivA); - } - } - btVector3 vTwist(1,0,0); - - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); - btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); - btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist - // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - int row = 3; - int srow = row * info->rowskip; - btVector3 ax1; - // angular limits - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - btTransform trA = transA*m_frameInA; - btVector3 twistAxis = trA.getBasis().getColumn(0); - - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps; - info->m_constraintError[srow] = fact * swingAxis.dot(p); - info->m_constraintError[srow1] = fact * swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - - { - btQuaternion qMinTwist = qABTwist; - btScalar twistAngle = qABTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = -(qABTwist); - twistAngle = qMinTwist.getAngle(); - } - - if (twistAngle > SIMD_EPSILON) - { - twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - twistAxis.normalize(); - twistAxis = quatRotate(qB, -twistAxis); - } - ax1 = twistAxis; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps; - info->m_constraintError[srow] = k * twistAngle; - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - } +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btFixedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + + +btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) +:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) +{ + m_frameInA = frameInA; + m_frameInB = frameInB; + +} + +btFixedConstraint::~btFixedConstraint () +{ +} + + +void btFixedConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 6; + info->nub = 0; +} + +void btFixedConstraint::getInfo2 (btConstraintInfo2* info) +{ + //fix the 3 linear degrees of freedom + + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + + const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); + const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); + const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); + const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); + + + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + // set right hand side for the linear dofs + btScalar k = info->fps * info->erp; + + btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = linearError[j]; + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); + btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); + btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); + btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); + btVector3 target; + //btScalar x = ivB.dot(ivA);//?? + btScalar y = ivB.dot(jvA); + btScalar z = ivB.dot(kvA); + btVector3 swingAxis(0,0,0); + { + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + { + swingAxis = -ivB.cross(ivA); + } + } + btVector3 vTwist(1,0,0); + + // compute rotation of A wrt B (in constraint space) + btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); + btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); + btQuaternion qAB = qB.inverse() * qA; + // split rotation into cone and twist + // (all this is done from B's perspective. Maybe I should be averaging axes...) + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + + int row = 3; + int srow = row * info->rowskip; + btVector3 ax1; + // angular limits + { + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + btTransform trA = transA*m_frameInA; + btVector3 twistAxis = trA.getBasis().getColumn(0); + + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + int srow1 = srow + info->rowskip; + J1[srow+0] = p[0]; + J1[srow+1] = p[1]; + J1[srow+2] = p[2]; + J1[srow1+0] = q[0]; + J1[srow1+1] = q[1]; + J1[srow1+2] = q[2]; + J2[srow+0] = -p[0]; + J2[srow+1] = -p[1]; + J2[srow+2] = -p[2]; + J2[srow1+0] = -q[0]; + J2[srow1+1] = -q[1]; + J2[srow1+2] = -q[2]; + btScalar fact = info->fps; + info->m_constraintError[srow] = fact * swingAxis.dot(p); + info->m_constraintError[srow1] = fact * swingAxis.dot(q); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_lowerLimit[srow1] = -SIMD_INFINITY; + info->m_upperLimit[srow1] = SIMD_INFINITY; + srow = srow1 + info->rowskip; + + { + btQuaternion qMinTwist = qABTwist; + btScalar twistAngle = qABTwist.getAngle(); + + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qMinTwist = -(qABTwist); + twistAngle = qMinTwist.getAngle(); + } + + if (twistAngle > SIMD_EPSILON) + { + twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + twistAxis.normalize(); + twistAxis = quatRotate(qB, -twistAxis); + } + ax1 = twistAxis; + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps; + info->m_constraintError[srow] = k * twistAngle; + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + } } \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index 386cf266b..e1c9430fd 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -1,48 +1,48 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FIXED_CONSTRAINT_H -#define BT_FIXED_CONSTRAINT_H - -#include "btTypedConstraint.h" - -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint -{ - - btTransform m_frameInA; - btTransform m_frameInB; -public: - btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); - - virtual ~btFixedConstraint(); - - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - virtual void setParam(int num, btScalar value, int axis = -1) - { - btAssert(0); - } - virtual btScalar getParam(int num, int axis = -1) const - { - btAssert(0); - return 0.f; - } - -}; - -#endif //BT_FIXED_CONSTRAINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_FIXED_CONSTRAINT_H +#define BT_FIXED_CONSTRAINT_H + +#include "btTypedConstraint.h" + +ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint +{ + + btTransform m_frameInA; + btTransform m_frameInB; +public: + btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); + + virtual ~btFixedConstraint(); + + + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void setParam(int num, btScalar value, int axis = -1) + { + btAssert(0); + } + virtual btScalar getParam(int num, int axis = -1) const + { + btAssert(0); + return 0.f; + } + +}; + +#endif //BT_FIXED_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp index 446e69de7..bcd457b67 100644 --- a/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -1,54 +1,54 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. - -#include "btGearConstraint.h" - -btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) -:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), -m_axisInA(axisInA), -m_axisInB(axisInB), -m_ratio(ratio) -{ -} - -btGearConstraint::~btGearConstraint () -{ -} - -void btGearConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 1; - info->nub = 1; -} - -void btGearConstraint::getInfo2 (btConstraintInfo2* info) -{ - btVector3 globalAxisA, globalAxisB; - - globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; - globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; - - info->m_J1angularAxis[0] = globalAxisA[0]; - info->m_J1angularAxis[1] = globalAxisA[1]; - info->m_J1angularAxis[2] = globalAxisA[2]; - - info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; - info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; - info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; - -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. + +#include "btGearConstraint.h" + +btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) +:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), +m_axisInA(axisInA), +m_axisInB(axisInB), +m_ratio(ratio) +{ +} + +btGearConstraint::~btGearConstraint () +{ +} + +void btGearConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 1; + info->nub = 1; +} + +void btGearConstraint::getInfo2 (btConstraintInfo2* info) +{ + btVector3 globalAxisA, globalAxisB; + + globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; + globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; + + info->m_J1angularAxis[0] = globalAxisA[0]; + info->m_J1angularAxis[1] = globalAxisA[1]; + info->m_J1angularAxis[2] = globalAxisA[2]; + + info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; + info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; + info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; + +} + diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h index e3c5dff35..f9afcb912 100644 --- a/src/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -1,79 +1,79 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_GEAR_CONSTRAINT_H -#define BT_GEAR_CONSTRAINT_H - -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btGearConstraintData btGearConstraintDoubleData -#define btGearConstraintDataName "btGearConstraintDoubleData" -#else -#define btGearConstraintData btGearConstraintFloatData -#define btGearConstraintDataName "btGearConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - - -///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. -///See Bullet/Demos/ConstraintDemo for an example use. -class btGearConstraint : public btTypedConstraint -{ -protected: - btVector3 m_axisInA; - btVector3 m_axisInB; - bool m_useFrameA; - btScalar m_ratio; - -public: - btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); - virtual ~btGearConstraint (); - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (btConstraintInfo1* info); - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef BT_GEAR_CONSTRAINT_H +#define BT_GEAR_CONSTRAINT_H + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGearConstraintData btGearConstraintDoubleData +#define btGearConstraintDataName "btGearConstraintDoubleData" +#else +#define btGearConstraintData btGearConstraintFloatData +#define btGearConstraintDataName "btGearConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + +///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. +///See Bullet/Demos/ConstraintDemo for an example use. +class btGearConstraint : public btTypedConstraint +{ +protected: + btVector3 m_axisInA; + btVector3 m_axisInB; + bool m_useFrameA; + btScalar m_ratio; + +public: + btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); + virtual ~btGearConstraint (); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo1 (btConstraintInfo1* info); + ///internal method used by the constraint solver, don't use them directly virtual void getInfo2 (btConstraintInfo2* info); - void setAxisA(btVector3& axisA) - { - m_axisInA = axisA; + void setAxisA(btVector3& axisA) + { + m_axisInA = axisA; } - void setAxisB(btVector3& axisB) - { - m_axisInB = axisB; + void setAxisB(btVector3& axisB) + { + m_axisInB = axisB; } - void setRatio(btScalar ratio) - { - m_ratio = ratio; + void setRatio(btScalar ratio) + { + m_ratio = ratio; } - const btVector3& getAxisA() const - { - return m_axisInA; + const btVector3& getAxisA() const + { + return m_axisInA; } - const btVector3& getAxisB() const - { - return m_axisInB; + const btVector3& getAxisB() const + { + return m_axisInB; } - btScalar getRatio() const - { - return m_ratio; + btScalar getRatio() const + { + return m_ratio; } @@ -84,69 +84,69 @@ public: (void) axis; btAssert(0); } - - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const - { - (void) num; - (void) axis; - btAssert(0); - return 0.f; - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -}; - - - - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGearConstraintFloatData -{ - btTypedConstraintFloatData m_typeConstraintData; - - btVector3FloatData m_axisInA; - btVector3FloatData m_axisInB; - - float m_ratio; - char m_padding[4]; -}; - -struct btGearConstraintDoubleData -{ - btTypedConstraintDoubleData m_typeConstraintData; - - btVector3DoubleData m_axisInA; - btVector3DoubleData m_axisInB; - - double m_ratio; -}; - -SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btGearConstraintData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; - btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); - - m_axisInA.serialize( gear->m_axisInA ); - m_axisInB.serialize( gear->m_axisInB ); - - gear->m_ratio = m_ratio; - - return btGearConstraintDataName; -} - - - - - - -#endif //BT_GEAR_CONSTRAINT_H + + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const + { + (void) num; + (void) axis; + btAssert(0); + return 0.f; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; +}; + + + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGearConstraintFloatData +{ + btTypedConstraintFloatData m_typeConstraintData; + + btVector3FloatData m_axisInA; + btVector3FloatData m_axisInB; + + float m_ratio; + char m_padding[4]; +}; + +struct btGearConstraintDoubleData +{ + btTypedConstraintDoubleData m_typeConstraintData; + + btVector3DoubleData m_axisInA; + btVector3DoubleData m_axisInB; + + double m_ratio; +}; + +SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGearConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; + btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); + + m_axisInA.serialize( gear->m_axisInA ); + m_axisInB.serialize( gear->m_axisInB ); + + gear->m_ratio = m_ratio; + + return btGearConstraintDataName; +} + + + + + + +#endif //BT_GEAR_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index 696aafb72..ee57ca72b 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -50,8 +50,8 @@ btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) , m_flags(0) - , m_rotateOrder(rotOrder) { calculateTransforms(); } @@ -60,8 +60,8 @@ btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) , m_frameInB(frameInB) - , m_flags(0) , m_rotateOrder(rotOrder) + , m_flags(0) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; @@ -776,7 +776,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( btScalar kd = limot->m_springDamping; btScalar ks = limot->m_springStiffness; btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); - btScalar erp = 0.1; +// btScalar erp = 0.1; btScalar cfm = 0.0; btScalar mA = 1.0 / m_rbA.getInvMass(); btScalar mB = 1.0 / m_rbB.getInvMass(); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 30d506c6e..e2aaee7ac 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -217,6 +217,14 @@ public: } + bool hasLimit() const { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHalfRange() > 0; +#else + return m_lowerLimit <= m_upperLimit; +#endif + } + btScalar getLowerLimit() const { #ifdef _BT_USE_CENTER_LIMIT_ diff --git a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp index 11fbadef7..f110cd480 100644 --- a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -1,463 +1,463 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btNNCGConstraintSolver.h" - - - - - - -btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); - - m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); - m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); - m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); - m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); - - m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); - m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); - m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); - m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); - - return val; -} - -btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) -{ - - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) - { - - for (int j=0; j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) - { - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - - ///solve all contact constraints using SIMD, if available - if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; - - for (int c=0;cbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[c*multiplier] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier] = 0; - } - } - - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) - { - - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[c*multiplier+1] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier+1] = 0; - } - } - } - } - - } - else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS - { - //solve the friction constraints after all contact constraints, don't interleave them - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - - for (j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[j] = 0; - } - } - - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCRF[j] = 0; - } - } - - - } - } - - - - } else - { - - if (iteration< infoGlobal.m_numIterations) - { - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[j] = 0; - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCRF[j] = 0; - } - } - } - } - - - - - if (!m_onlyForNoneContact) - { - if (iteration==0) - { - for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) { - for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) + { + for (int j=0;jisEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + + ///solve all contact constraints using SIMD, if available + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + + for (int c=0;cbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier] = 0; + } + } + + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier+1] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier+1] = 0; + } + } + } + } + + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + + for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + + + } + } + + + + } else + { + + if (iteration< infoGlobal.m_numIterations) + { + for (int j=0;jisEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (int j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + } + } + + + + + if (!m_onlyForNoneContact) + { + if (iteration==0) + { + for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) { + for (int j=0;j m_pNC; // p for None Contact constraints - btAlignedObjectArray m_pC; // p for Contact constraints - btAlignedObjectArray m_pCF; // p for ContactFriction constraints - btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints - - //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. - btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints - btAlignedObjectArray m_deltafC; // deltaf for Contact constraints - btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints - btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints - - -protected: - - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} - - virtual btConstraintSolverType getSolverType() const - { - return BT_NNCG_SOLVER; - } - - bool m_onlyForNoneContact; -}; - - - - -#endif //BT_NNCG_CONSTRAINT_SOLVER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_NNCG_CONSTRAINT_SOLVER_H +#define BT_NNCG_CONSTRAINT_SOLVER_H + +#include "btSequentialImpulseConstraintSolver.h" + +ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver +{ +protected: + + btScalar m_deltafLengthSqrPrev; + + btAlignedObjectArray m_pNC; // p for None Contact constraints + btAlignedObjectArray m_pC; // p for Contact constraints + btAlignedObjectArray m_pCF; // p for ContactFriction constraints + btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints + + //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. + btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints + btAlignedObjectArray m_deltafC; // deltaf for Contact constraints + btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints + btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints + + +protected: + + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} + + virtual btConstraintSolverType getSolverType() const + { + return BT_NNCG_SOLVER; + } + + bool m_onlyForNoneContact; +}; + + + + +#endif //BT_NNCG_CONSTRAINT_SOLVER_H + diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 8385639a1..fb06b76c3 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -364,9 +364,9 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() - :m_btSeed2(0), - m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), - m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference) + : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), + m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference), + m_btSeed2(0) { #ifdef USE_SIMD @@ -761,8 +761,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra const btVector3& rel_pos1, const btVector3& rel_pos2) { - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index a6a51662d..409b64e96 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -58,7 +58,7 @@ int firstHit=startHit; SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; - + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); @@ -88,7 +88,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal int m_numConstraints; btIDebugDraw* m_debugDrawer; btDispatcher* m_dispatcher; - + btAlignedObjectArray m_bodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray m_constraints; @@ -127,7 +127,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal m_constraints.resize (0); } - + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { if (islandId<0) @@ -140,7 +140,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal btTypedConstraint** startConstraint = 0; int numCurConstraints = 0; int i; - + //find the first constraint for this island for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); } else { - + for (i=0;isolveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); @@ -206,10 +206,10 @@ m_solverIslandCallback ( NULL ), m_constraintSolver(constraintSolver), m_gravity(0,-10,0), m_localTime(0), +m_fixedTimeStep(0), m_synchronizeAllMotionStates(false), m_applySpeculativeContactRestitution(false), m_profileTimings(0), -m_fixedTimeStep(0), m_latencyMotionStateInterpolation(true) { @@ -332,7 +332,7 @@ void btDiscreteDynamicsWorld::clearForces() //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up body->clearForces(); } -} +} ///apply gravity, call this once per timestep void btDiscreteDynamicsWorld::applyGravity() @@ -448,7 +448,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, applyGravity(); - + for (int i=0;iupdateAction( this, timeStep); } } - - + + void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); @@ -637,7 +637,7 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { if (body->getActivationState() == ACTIVE_TAG) body->setActivationState( WANTS_DEACTIVATION ); - if (body->getActivationState() == ISLAND_SLEEPING) + if (body->getActivationState() == ISLAND_SLEEPING) { body->setAngularVelocity(btVector3(0,0,0)); body->setLinearVelocity(btVector3(0,0,0)); @@ -710,25 +710,25 @@ void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - + m_sortedConstraints.resize( m_constraints.size()); - int i; + int i; for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - + /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); @@ -749,10 +749,10 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() for (int i=0;im_predictiveManifolds.size();i++) { btPersistentManifold* manifold = m_predictiveManifolds[i]; - + const btCollisionObject* colObj0 = manifold->getBody0(); const btCollisionObject* colObj1 = manifold->getBody1(); - + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { @@ -760,7 +760,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() } } } - + { int i; int numConstraints = int(m_constraints.size()); @@ -784,7 +784,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - + } @@ -800,7 +800,7 @@ public: btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), m_me(me), m_allowedPenetration(0.0f), @@ -880,7 +880,7 @@ int gNumClampedCcdMotions=0; void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) { BT_PROFILE("createPredictiveContacts"); - + { BT_PROFILE("release predictive contact manifolds"); @@ -902,7 +902,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) { body->predictIntegratedTransform(timeStep, predictedTrans); - + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) @@ -916,7 +916,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) { public: - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) { } @@ -946,14 +946,14 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - + btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); - + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); m_predictiveManifolds.push_back(manifold); - + btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; @@ -986,10 +986,10 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { body->predictIntegratedTransform(timeStep, predictedTrans); - + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - + if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -1002,7 +1002,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { public: - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) { } @@ -1032,7 +1032,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - + //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); @@ -1057,13 +1057,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) printf("sm2=%f\n",sm2); } #else - + //don't apply the collision response right now, it will happen next frame //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution. //btScalar appliedImpulse = 0.f; //btScalar depth = 0.f; //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - + #endif @@ -1071,10 +1071,10 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } } - + body->proceedToTransform( predictedTrans); - + } } @@ -1088,7 +1088,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btPersistentManifold* manifold = m_predictiveManifolds[i]; btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0()); btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1()); - + for (int p=0;pgetNumContacts();p++) { const btManifoldPoint& pt = manifold->getContactPoint(p); @@ -1098,11 +1098,11 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) { btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution; - + const btVector3& pos1 = pt.getPositionWorldOnA(); const btVector3& pos2 = pt.getPositionWorldOnB(); - btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); + btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); if (body0) @@ -1113,7 +1113,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } } - + } @@ -1152,7 +1152,7 @@ void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) - + void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) { @@ -1172,12 +1172,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btTransform tr; tr.setIdentity(); btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; tr.setOrigin(pivot); getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame + // that ideally should draw the same frame pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; tr.setOrigin(pivot); if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); } @@ -1196,13 +1196,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } bool drawSect = true; - if(minAng > maxAng) + if(!pHinge->hasLimit()) { minAng = btScalar(0.f); maxAng = SIMD_2_PI; drawSect = false; } - if(drawLimits) + if(drawLimits) { btVector3& center = tr.getOrigin(); btVector3 normal = tr.getBasis().getColumn(2); @@ -1237,7 +1237,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); pPrev = pCur; - } + } btScalar tws = pCT->getTwistSpan(); btScalar twa = pCT->getTwistAngle(); bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); @@ -1265,7 +1265,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); tr = p6DOF->getCalculatedTransformB(); if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + if(drawLimits) { tr = p6DOF->getCalculatedTransformA(); const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); @@ -1328,7 +1328,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) } } break; - default : + default : break; } return; @@ -1428,19 +1428,19 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; - + worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; - + worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; - + worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; - + #ifdef BT_USE_DOUBLE_PRECISION const char* structType = "btDynamicsWorldDoubleData"; #else//BT_USE_DOUBLE_PRECISION diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index fc12bfd6b..2e930191e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -87,7 +87,10 @@ btMultiBody::btMultiBody(int n_links, bool fixedBase, bool canSleep, bool multiDof) - : m_baseQuat(0, 0, 0, 1), + : + m_baseCollider(0), + m_basePos(0,0,0), + m_baseQuat(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -95,18 +98,19 @@ btMultiBody::btMultiBody(int n_links, m_awake(true), m_canSleep(canSleep), m_sleepTimer(0), - m_baseCollider(0), + m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), + m_maxAppliedImpulse(1000.f), m_maxCoordinateVelocity(100.f), - m_hasSelfCollision(true), - m_dofCount(0), - __posUpdated(false), + m_hasSelfCollision(true), m_isMultiDof(multiDof), + __posUpdated(false), + m_dofCount(0), m_posVarCnt(0), - m_useRK4(false), m_useGlobalVelocities(false) + m_useRK4(false), + m_useGlobalVelocities(false) { if(!m_isMultiDof) @@ -119,7 +123,7 @@ btMultiBody::btMultiBody(int n_links, m_links.resize(n_links); m_matrixBuf.resize(n_links + 1); - m_basePos.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); m_baseTorque.setValue(0, 0, 0); } @@ -128,6 +132,38 @@ btMultiBody::~btMultiBody() { } +void btMultiBody::setupFixed(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision) +{ + btAssert(m_isMultiDof); + + m_links[i].m_mass = mass; + m_links[i].m_inertia = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_eVector = parentComToThisComOffset; + + m_links[i].m_jointType = btMultibodyLink::eFixed; + m_links[i].m_dofCount = 0; + m_links[i].m_posVarCount = 0; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + //if(m_isMultiDof) + // resizeInternalMultiDofBuffers(); + // + updateLinksDofOffsets(); + +} + void btMultiBody::setupPrismatic(int i, btScalar mass, const btVector3 &inertia, @@ -286,12 +322,15 @@ void btMultiBody::setupPlanar(int i, m_links[i].m_jointType = btMultibodyLink::ePlanar; m_links[i].m_dofCount = 3; m_links[i].m_posVarCount = 3; - m_links[i].getAxisTop(0) = rotationAxis.normalized(); - m_links[i].getAxisTop(1).setZero(); - m_links[i].getAxisTop(2).setZero(); - m_links[i].getAxisBottom(0).setZero(); - m_links[i].getAxisBottom(1) = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); - m_links[i].getAxisBottom(2) = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); + btVector3 n=rotationAxis.normalized(); + m_links[i].setAxisTop(0, n[0],n[1],n[2]); + m_links[i].setAxisTop(1,0,0,0); + m_links[i].setAxisTop(2,0,0,0); + m_links[i].setAxisBottom(0,0,0,0); + btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); + m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); + cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); + m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; @@ -1167,41 +1206,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; fromWorld.m_rotMat = rot_from_world[i+1]; - - ////clamp parent's omega - //btScalar parOmegaMod = spatVel[parent+1].getAngular().length(); - //btScalar parOmegaModMax = 0.1; - //if(parOmegaMod > parOmegaModMax) - //{ - // //btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear()); - // btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod); - // fromParent.transform(clampedParVel, spatVel[i+1]); - // spatVel[parent+1] *= (parOmegaModMax / parOmegaMod); - //} - //else - { - // vhat_i = i_xhat_p(i) * vhat_p(i) - fromParent.transform(spatVel[parent+1], spatVel[i+1]); - //nice alternative below (using operator *) but it generates temps - } - ////////////////////////////////////////////////////////////// - - //if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical) - //{ - // btScalar mod2 = 0; - // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof]; - - // btScalar angvel = sqrt(mod2); - // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; - // btScalar step = 1; //dt - // if (angvel*step > maxAngVel) - // { - // btScalar * qd = getJointVelMultiDof(i); - // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // qd[dof] *= (maxAngVel/step) /angvel; - // } - //} + fromParent.transform(spatVel[parent+1], spatVel[i+1]); // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i @@ -1257,7 +1262,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular())); // zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); - btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); ////clamp parent's omega //btScalar parOmegaMod = temp.length(); //btScalar parOmegaModMax = 1000; @@ -1713,13 +1718,14 @@ void btMultiBody::stepVelocities(btScalar dt, h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0); btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]); D[i] = val; + Y[i] = m_links[i].m_jointTorque[0] - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); const int parent = m_links[i].m_parent; - + btAssert(D[i]!=0.f); // Ip += pXi * (Ii - hi hi' / Di) * iXp const btScalar one_over_di = 1.0f / D[i]; @@ -2010,7 +2016,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar { for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]); +//?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]); Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]) @@ -2352,7 +2358,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // 'Downward' loop. for (int i = num_links - 1; i >= 0; --i) { - btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); +// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] += force[6 + i]; // add joint torque @@ -2437,7 +2443,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output ///////////////// */ - int dummy = 0; + //int dummy = 0; } void btMultiBody::stepPositions(btScalar dt) @@ -2633,7 +2639,7 @@ void btMultiBody::filConstraintJacobianMultiDof(int link, btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); scratch_r.resize(m_dofCount); - btScalar * results = num_links > 0 ? &scratch_r[0] : 0; + btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; btMatrix3x3 * rot_from_world = &scratch_m[0]; @@ -2713,9 +2719,9 @@ void btMultiBody::filConstraintJacobianMultiDof(int link, #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS case btMultibodyLink::ePlanar: { - results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2)); + results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); break; } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 85d6a2f3f..97825d7b4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -56,7 +56,16 @@ public: ~btMultiBody(); - void setupPrismatic(int i, // 0 to num_links-1 + void setupFixed(int linkIndex, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision); + + + void setupPrismatic(int linkIndex, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, // in my frame; assumed diagonal int parent, @@ -66,17 +75,17 @@ public: bool disableParentCollision=false ); - void setupRevolute(int i, // 0 to num_links-1 + void setupRevolute(int linkIndex, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, - int parent, + int parentIndex, const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 const btVector3 &jointAxis, // in my frame const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame bool disableParentCollision=false); - void setupSpherical(int i, // 0 to num_links-1 + void setupSpherical(int linkIndex, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, int parent, @@ -510,8 +519,15 @@ public: void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } - bool __posUpdated; - + bool isPosUpdated() const + { + return __posUpdated; + } + void setPosUpdated(bool updated) + { + __posUpdated = updated; + } + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -535,6 +551,7 @@ private: void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + private: btMultiBodyLinkCollider* m_baseCollider;//can be NULL @@ -550,9 +567,7 @@ private: btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; - int m_dofCount, m_posVarCnt; - bool m_useRK4, m_useGlobalVelocities; // // realBuf: @@ -596,6 +611,9 @@ private: btScalar m_maxCoordinateVelocity; bool m_hasSelfCollision; bool m_isMultiDof; + bool __posUpdated; + int m_dofCount, m_posVarCnt; + bool m_useRK4, m_useGlobalVelocities; }; #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 1bb518f94..3c71a08b9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,364 +1,366 @@ -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100), - m_jacSizeA(0), - m_jacSizeBoth(0) -{ - - if(bodyA) - { - if(bodyA->isMultiDof()) - m_jacSizeA = (6 + bodyA->getNumDofs()); - else - m_jacSizeA = (6 + bodyA->getNumLinks()); - } - - if(bodyB) - { - if(bodyB->isMultiDof()) - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); - else - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); - } - else - m_jacSizeBoth = m_jacSizeA; - - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); - m_data.resize((2 + m_jacSizeBoth) * m_numRows); -} - -btMultiBodyConstraint::~btMultiBodyConstraint() -{ -} - -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - solverConstraint.m_multiBodyA = m_bodyA; - solverConstraint.m_multiBodyB = m_bodyB; - solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary - if (bodyA) - rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom - //resize.. - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - //copy/determine - if(jacOrgA) - { - for (int i=0;iisMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - //determine.. - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - else if(rb0) - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom - //resize.. - solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - //copy/determine.. - if(jacOrgB) - { - for (int i=0;iisMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - //determine.. - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - - } - else if(rb1) - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* deltaVelA = 0; - btScalar* deltaVelB = 0; - int ndofA = 0; - //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l = deltaVelA[i]; - denom0 += j*l; - } - } - else if(rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - // - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l = deltaVelB[i]; - denom1 += j*l; - } - - } - else if(rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * deltaVelA[i]; - denom1 += jacA[i] * deltaVelB[i]; - } - } - // - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } - else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - } - - - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if(rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - else if(rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - -} +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) + + + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_numRows(numRows), + m_jacSizeA(0), + m_jacSizeBoth(0), + m_isUnilateral(isUnilateral), + m_maxAppliedImpulse(100) +{ + + if(bodyA) + { + if(bodyA->isMultiDof()) + m_jacSizeA = (6 + bodyA->getNumDofs()); + else + m_jacSizeA = (6 + bodyA->getNumLinks()); + } + + if(bodyB) + { + if(bodyB->isMultiDof()) + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); + else + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); + } + else + m_jacSizeBoth = m_jacSizeA; + + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); +} + +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + if (bodyA) + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); + + if (multiBodyA) + { + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom + //resize.. + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + //copy/determine + if(jacOrgA) + { + for (int i=0;iisMultiDof()) + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + //determine.. + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + else if(rb0) + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom + //resize.. + solverConstraint.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + //copy/determine.. + if(jacOrgB) + { + for (int i=0;iisMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + //determine.. + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + + } + else if(rb1) + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 0; + int ndofA = 0; + //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l = deltaVelA[i]; + denom0 += j*l; + } + } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + // + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l = deltaVelB[i]; + denom1 += j*l; + } + + } + else if(rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * deltaVelA[i]; + denom1 += jacA[i] * deltaVelB[i]; + } + } + // + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + } + + + //compute rhs and remaining solverConstraint fields + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if(rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + if (multiBodyB) + { + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + else if(rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + positionalError = -penetration * erp/infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index d01870485..454c9c380 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -1,160 +1,160 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_H -#define BT_MULTIBODY_CONSTRAINT_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btMultiBody.h" - -class btMultiBody; -struct btSolverInfo; - -#include "btMultiBodySolverConstraint.h" - -struct btMultiBodyJacobianData -{ - btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension - btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - btAlignedObjectArray* m_solverBodyPool; - int m_fixedBodyId; - -}; - - -class btMultiBodyConstraint -{ -protected: - - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; - - int m_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; - - bool m_isUnilateral; - - btScalar m_maxAppliedImpulse; - - - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray m_data; - - void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); - -public: - - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); - virtual ~btMultiBodyConstraint(); - - - - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; - - int getNumRows() const - { - return m_numRows; - } - - btMultiBody* getMultiBodyA() - { - return m_bodyA; - } - btMultiBody* getMultiBodyB() - { - return m_bodyB; - } - - // current constraint position - // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral - // NOTE: ignored position for friction rows. - btScalar getPosition(int row) const - { - return m_data[m_posOffset + row]; - } - - void setPosition(int row, btScalar pos) - { - m_data[m_posOffset + row] = pos; - } - - - bool isUnilateral() const - { - return m_isUnilateral; - } - - // jacobian blocks. - // each of size 6 + num_links. (jacobian2 is null if no body2.) - // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. - btScalar* jacobianA(int row) - { - return &m_data[m_numRows + row * m_jacSizeBoth]; - } - const btScalar* jacobianA(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth)]; - } - btScalar* jacobianB(int row) - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - const btScalar* jacobianB(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - - -}; - -#endif //BT_MULTIBODY_CONSTRAINT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_CONSTRAINT_H +#define BT_MULTIBODY_CONSTRAINT_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btMultiBody.h" + +class btMultiBody; +struct btSolverInfo; + +#include "btMultiBodySolverConstraint.h" + +struct btMultiBodyJacobianData +{ + btAlignedObjectArray m_jacobians; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension + btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + btAlignedObjectArray* m_solverBodyPool; + int m_fixedBodyId; + +}; + + +class btMultiBodyConstraint +{ +protected: + + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_numRows; + int m_jacSizeA; + int m_jacSizeBoth; + int m_posOffset; + + bool m_isUnilateral; + + btScalar m_maxAppliedImpulse; + + + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray m_data; + + void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); + + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); + +public: + + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); + virtual ~btMultiBodyConstraint(); + + + + virtual int getIslandIdA() const =0; + virtual int getIslandIdB() const =0; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal)=0; + + int getNumRows() const + { + return m_numRows; + } + + btMultiBody* getMultiBodyA() + { + return m_bodyA; + } + btMultiBody* getMultiBodyB() + { + return m_bodyB; + } + + // current constraint position + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: ignored position for friction rows. + btScalar getPosition(int row) const + { + return m_data[m_posOffset + row]; + } + + void setPosition(int row, btScalar pos) + { + m_data[m_posOffset + row] = pos; + } + + + bool isUnilateral() const + { + return m_isUnilateral; + } + + // jacobian blocks. + // each of size 6 + num_links. (jacobian2 is null if no body2.) + // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. + btScalar* jacobianA(int row) + { + return &m_data[m_numRows + row * m_jacSizeBoth]; + } + const btScalar* jacobianA(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth)]; + } + btScalar* jacobianB(int row) + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + } + const btScalar* jacobianB(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + +}; + +#endif //BT_MULTIBODY_CONSTRAINT_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 90a0e25a5..7f77376bf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -1,906 +1,907 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "btMultiBodyLinkCollider.h" - -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -#include "LinearMath/btQuickprof.h" - -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - - //solve featherstone non-contact constraints - - //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j=0;j__posUpdated = false; - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->__posUpdated = false; - } - - //solve featherstone normal contact - for (int j=0;j__posUpdated = false; - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->__posUpdated = false; - } - - //solve featherstone frictional contact - - for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) - { - if (iteration < infoGlobal.m_numIterations) - { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - resolveSingleConstraintRowGeneric(frictionConstraint); - - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->__posUpdated = false; - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->__posUpdated = false; - } - } - } - return val; -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - m_multiBodyNonContactConstraints.resize(0); - m_multiBodyNormalContactConstraints.resize(0); - m_multiBodyFrictionContactConstraints.resize(0); - m_data.m_jacobians.resize(0); - m_data.m_deltaVelocitiesUnitImpulse.resize(0); - m_data.m_deltaVelocities.resize(0); - - for (int i=0;im_multiBody->setCompanionId(-1); - } - } - - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); - - return val; -} - -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - btSolverBody* bodyA = 0; - btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else if(c.m_solverBodyIdA >= 0) - { - bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (c.m_multiBodyB) - { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else if(c.m_solverBodyIdB >= 0) - { - bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); - } - - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyA->isMultiDof()) - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - else - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } else if(c.m_solverBodyIdA >= 0) - { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyB->isMultiDof()) - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - else - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } else if(c.m_solverBodyIdB >= 0) - { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } - -} - - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } - - if (c.m_multiBodyB) - { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } - - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } -} - - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - BT_PROFILE("setupMultiBodyContactConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormal; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); - } - - solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - if(multiBodyB->isMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormal; - } - - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } else - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormal.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } else - { - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormal.dot(vec); - } - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else - { - if (rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else - { - if (rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - } - - solverConstraint.m_friction = cp.m_combinedFriction; - - if(!isFriction) - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - } - } - } - - - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - if(multiBodyB->isMultiDof()) - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) - { - positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if(!isFriction) - { - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - else - { - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; - } - - solverConstraint.m_cfm = 0.f; //why not use cfmSlip? - } - -} - - - - -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("addMultiBodyFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); - return solverConstraint; -} - -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) -{ - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - - btCollisionObject* colObj0=0,*colObj1=0; - - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - - btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; - - - ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) - // return; - - int rollingFriction=1; - - for (int j=0;jgetNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - - btScalar relaxation; - - int frictionIndex = m_multiBodyNormalContactConstraints.size(); - - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints -#define ENABLE_FRICTION -#ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION - - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame - ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. - - ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. - ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. - /// - ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. - /// - ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true - ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) - ///this will give a conveyor belt effect - /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - {/* - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - */ - { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_lateralFrictionInitialized = true; - } - } - - } else - { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } - - -#endif //ENABLE_FRICTION - - } - } -} - -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) -{ - btPersistentManifold* manifold = 0; - - for (int i=0;igetBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - if (!fcA && !fcB) - { - //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold,infoGlobal); - } else - { - convertMultiBodyContact(manifold,infoGlobal); - } - } - - //also convert the multibody constraints, if any - - - for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); - } - -} - - - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) -{ - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) -{ - int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; - - pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse; - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - //do a callback here? - } - } - - - numPoolConstraints = m_multiBodyNonContactConstraints.size(); - -#if 0 - //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint - for (int i=0;igetJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - - } - - constr->internalSetAppliedImpulse(c.m_appliedImpulse); - if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - - } -#endif - - - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); -} - - -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) -{ - //printf("solveMultiBodyGroup start\n"); - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; - - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiBodyConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "btMultiBodyLinkCollider.h" + +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +#include "LinearMath/btQuickprof.h" + +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + //solve featherstone non-contact constraints + + //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;jsetPosUpdated(false); + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } + + //solve featherstone normal contact + for (int j=0;jsetPosUpdated(false); + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } + + //solve featherstone frictional contact + + for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) + { + if (iteration < infoGlobal.m_numIterations) + { + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + resolveSingleConstraintRowGeneric(frictionConstraint); + + if(frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if(frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + return val; +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + m_multiBodyNonContactConstraints.resize(0); + m_multiBodyNormalContactConstraints.resize(0); + m_multiBodyFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); + m_data.m_deltaVelocitiesUnitImpulse.resize(0); + m_data.m_deltaVelocities.resize(0); + + for (int i=0;im_multiBody->setCompanionId(-1); + } + } + + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + + return val; +} + +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + int ndofA=0; + int ndofB=0; + + if (c.m_multiBodyA) + { + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; + } else if(c.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (c.m_multiBodyB) + { + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; + } else if(c.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + if(c.m_multiBodyA->isMultiDof()) + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + else + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } else if(c.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + if(c.m_multiBodyB->isMultiDof()) + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + else + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } else if(c.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + +} + + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + int ndofA=0; + int ndofB=0; + + if (c.m_multiBodyA) + { + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; + } + + if (c.m_multiBodyB) + { + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } +} + + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyContactConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormal; + } + + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + if(multiBodyB->isMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormal; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormal.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormal.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + } + + solverConstraint.m_friction = cp.m_combinedFriction; + + if(!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + + ///warm starting (or zero if disabled) + //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) + if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + if(multiBodyB->isMultiDof()) + multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if(!isFriction) + { + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + else + { + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + } + + solverConstraint.m_cfm = 0.f; //why not use cfmSlip? + } + +} + + + + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyFrictionConstraint"); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + btCollisionObject* colObj0=0,*colObj1=0; + + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + +// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; +// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + + + ///avoid collision response between two static objects +// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // return; + + + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + + btScalar relaxation; + + int frictionIndex = m_multiBodyNormalContactConstraints.size(); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); + + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + bool isFriction = false; + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints +#define ENABLE_FRICTION +#ifdef ENABLE_FRICTION + solverConstraint.m_frictionIndex = frictionIndex; +#if ROLLING_FRICTION + int rollingFriction=1; + btVector3 angVelA(0,0,0),angVelB(0,0,0); + if (rb0) + angVelA = rb0->getAngularVelocity(); + if (rb1) + angVelB = rb1->getAngularVelocity(); + btVector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + btVector3 axis0,axis1; + btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length()>0.001) + addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (axis1.length()>0.001) + addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + } +#endif //ROLLING_FRICTION + + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + {/* + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + */ + { + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionInitialized = true; + } + } + + } else + { + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + + //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + //todo: + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + } + + +#endif //ENABLE_FRICTION + + } + } +} + +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + //btPersistentManifold* manifold = 0; + + for (int i=0;igetBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (!fcA && !fcB) + { + //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case + convertContact(manifold,infoGlobal); + } else + { + convertMultiBodyContact(manifold,infoGlobal); + } + } + + //also convert the multibody constraints, if any + + + for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); + } + +} + + + +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); + int j; + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + + pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? + } + } + + + numPoolConstraints = m_multiBodyNonContactConstraints.size(); + +#if 0 + //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint + for (int i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ + + } + + constr->internalSetAppliedImpulse(c.m_appliedImpulse); + if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + + } +#endif + + + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); +} + + +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + //printf("solveMultiBodyGroup start\n"); + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; + + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 800941c64..f11a007aa 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -1,86 +1,86 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "btMultiBodySolverConstraint.h" - - -class btMultiBody; - -#include "btMultiBodyConstraint.h" - - - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver -{ - -protected: - - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - - btMultiBodyJacobianData m_data; - - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; - - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); - - void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); - - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - - virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); -}; - - - - - -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "btMultiBodySolverConstraint.h" + + +class btMultiBody; + +#include "btMultiBodyConstraint.h" + + + +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + + btMultiBodyJacobianData m_data; + + //temp storage for multi body constraints for a specific island/group called by 'solveGroup' + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; + + void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); + + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + + virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); +}; + + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 58e48cd9a..af280d24a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -1,753 +1,757 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyDynamicsWorld.h" -#include "btMultiBodyConstraintSolver.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btQuickprof.h" -#include "btMultiBodyConstraint.h" - - - - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) -{ - m_multiBodies.push_back(body); - -} - -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) -{ - m_multiBodies.remove(body); -} - -void btMultiBodyDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - //merge islands based on speculative contact manifolds too - for (int i=0;im_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; - - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - } - - //merge islands linked by Featherstone link colliders - for (int i=0;igetBaseCollider(); - - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - - if (((cur) && (!(cur)->isStaticOrKinematicObject())) && - ((prev) && (!(prev)->isStaticOrKinematicObject()))) - { - int tagPrev = prev->getIslandTag(); - int tagCur = cur->getIslandTag(); - getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); - } - if (cur && !cur->isStaticOrKinematicObject()) - prev = cur; - - } - } - } - - //merge islands linked by multibody constraints - { - for (int i=0;im_multiBodyConstraints.size();i++) - { - btMultiBodyConstraint* c = m_multiBodyConstraints[i]; - int tagA = c->getIslandIdA(); - int tagB = c->getIslandIdB(); - if (tagA>=0 && tagB>=0) - getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - -} - - -void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - - - for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); - if (!body->isAwake()) - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - } - } else - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - } - } - - } - } - - btDiscreteDynamicsWorld::updateActivationState(timeStep); -} - - -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate2 -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - - - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId= islandTagA>=0?islandTagA:islandTagB; - return islandId; - -} - - -class btSortMultiBodyConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray m_bodies; - btAlignedObjectArray m_manifolds; - btAlignedObjectArray m_constraints; - btAlignedObjectArray m_multiBodyConstraints; - - - MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); - m_multiBodyConstraints.resize(0); - } - - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i=0;im_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - -}; - - - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) - :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) -{ - //split impulse is not yet supported for Featherstone hierarchies - getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); -} - -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () -{ - delete m_solverMultiBodyIslandCallback; -} - -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - - - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); - - - { - BT_PROFILE("btMultiBody addForce and stepVelocities"); - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - - bod->clearForcesAndTorques(); - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - - bool doNotUpdatePos = false; - - if(bod->isMultiDof()) - { - if(!bod->isUsingRK4Integration()) - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); - //convenience - btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += numPosVars; - btScalar *scratch_qx = pMem; pMem += numPosVars; - btScalar *scratch_qd0 = pMem; pMem += numDofs; - btScalar *scratch_qd1 = pMem; pMem += numDofs; - btScalar *scratch_qd2 = pMem; pMem += numDofs; - btScalar *scratch_qd3 = pMem; pMem += numDofs; - btScalar *scratch_qdd0 = pMem; pMem += numDofs; - btScalar *scratch_qdd1 = pMem; pMem += numDofs; - btScalar *scratch_qdd2 = pMem; pMem += numDofs; - btScalar *scratch_qdd3 = pMem; pMem += numDofs; - btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for(int link = 0; link < bod->getNumLinks(); ++link) - { - for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for(int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody *bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) - { - for(int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } - - } pEulerIntegrate; - // - struct - { - void operator()(btMultiBody *pBody, const btScalar *pData) - { - btScalar *pVel = const_cast(pBody->getVelocityVector()); - - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - - } - } pCopyToVelocityVector; - // - struct - { - void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) - { - for(int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; - #define output &scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray delta_q; delta_q.resize(numDofs); - btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); - for(int i = 0; i < numDofs; ++i) - { - delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if(!doNotUpdatePos) - { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - - for(int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->__posUpdated = true; - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for(int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); - } - - } - } - else - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } - } - } - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - -} - -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btDiscreteDynamicsWorld::integrateTransforms(timeStep); - - { - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - - for (int b=0;bgetBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - - if (!isSleeping) - { - int nLinks = bod->getNumLinks(); - - ///base + num m_links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - - if(bod->isMultiDof()) - { - if(!bod->__posUpdated) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->__posUpdated = false; - } - } - else - bod->stepPositions(timeStep); - - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - - if (bod->getBaseCollider()) - { - btVector3 posr = local_origin[0]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float 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])); - - bod->getBaseCollider()->setWorldTransform(tr); - - } - - 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++) - { - btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link+1; - - btVector3 posr = local_origin[index]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float 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])); - - col->setWorldTransform(tr); - } - } - } else - { - bod->clearVelocities(); - } - } - } -} - - - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.push_back(constraint); -} - -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.remove(constraint); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btQuickprof.h" +#include "btMultiBodyConstraint.h" + + + + +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +{ + m_multiBodies.push_back(body); + +} + +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +{ + m_multiBodies.remove(body); +} + +void btMultiBodyDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i=0;im_predictiveManifolds.size();i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + if (constraint->isEnabled()) + { + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + } + + //merge islands linked by Featherstone link colliders + for (int i=0;igetBaseCollider(); + + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; + + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && + ((prev) && (!(prev)->isStaticOrKinematicObject()))) + { + int tagPrev = prev->getIslandTag(); + int tagCur = cur->getIslandTag(); + getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); + } + if (cur && !cur->isStaticOrKinematicObject()) + prev = cur; + + } + } + } + + //merge islands linked by multibody constraints + { + for (int i=0;im_multiBodyConstraints.size();i++) + { + btMultiBodyConstraint* c = m_multiBodyConstraints[i]; + int tagA = c->getIslandIdA(); + int tagB = c->getIslandIdB(); + if (tagA>=0 && tagB>=0) + getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + +} + + +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); + + + + for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); + if (!body->isAwake()) + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + } + } else + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + } + } + + } + } + + btDiscreteDynamicsWorld::updateActivationState(timeStep); +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate2 +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId= islandTagA>=0?islandTagA:islandTagB; + return islandId; + +} + + +class btSortMultiBodyConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray m_bodies; + btAlignedObjectArray m_manifolds; + btAlignedObjectArray m_constraints; + btAlignedObjectArray m_multiBodyConstraints; + + + MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + + } + + MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + m_multiBodyConstraints.resize(0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i=0;im_minimumSolverBatchSize<=1) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + + for (i=0;im_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + + btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; + btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; + btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + +}; + + + +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) + :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) +{ + //split impulse is not yet supported for Featherstone hierarchies + getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +{ + delete m_solverMultiBodyIslandCallback; +} + +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + + + BT_PROFILE("solveConstraints"); + + m_sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); + + + { + BT_PROFILE("btMultiBody addForce and stepVelocities"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + + bool doNotUpdatePos = false; + + if(bod->isMultiDof()) + { + if(!bod->isUsingRK4Integration()) + { + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + else + { + // + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); + //convenience + btScalar *pMem = &scratch_r2[0]; + btScalar *scratch_q0 = pMem; pMem += numPosVars; + btScalar *scratch_qx = pMem; pMem += numPosVars; + btScalar *scratch_qd0 = pMem; pMem += numDofs; + btScalar *scratch_qd1 = pMem; pMem += numDofs; + btScalar *scratch_qd2 = pMem; pMem += numDofs; + btScalar *scratch_qd3 = pMem; pMem += numDofs; + btScalar *scratch_qdd0 = pMem; pMem += numDofs; + btScalar *scratch_qdd1 = pMem; pMem += numDofs; + btScalar *scratch_qdd2 = pMem; pMem += numDofs; + btScalar *scratch_qdd3 = pMem; pMem += numDofs; + btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); + + ///// + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // + for(int link = 0; link < bod->getNumLinks(); ++link) + { + for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + } + // + for(int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// + struct + { + btMultiBody *bod; + btScalar *scratch_qx, *scratch_q0; + + void operator()() + { + for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) + { + for(int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } + + } pEulerIntegrate; + // + struct + { + void operator()(btMultiBody *pBody, const btScalar *pData) + { + btScalar *pVel = const_cast(pBody->getVelocityVector()); + + for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) + { + for(int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; + #define output &scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd0, 0, numDofs); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd1, 0, numDofs); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd2, 0, numDofs); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd3, 0, numDofs); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; delta_q.resize(numDofs); + btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); + for(int i = 0; i < numDofs; ++i) + { + delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; + } + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + if(!doNotUpdatePos) + { + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + for(int i = 0; i < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); + } + + //ugly hack which resets the cached data to t0 (needed for constraint solver) + { + for(int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); + } + + } + } + else//if(bod->isMultiDof()) + { + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + bod->clearForcesAndTorques(); + }//if (!isSleeping) + } + } + + m_solverMultiBodyIslandCallback->processConstraints(); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + +} + +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + btDiscreteDynamicsWorld::integrateTransforms(timeStep); + + { + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + + for (int b=0;bgetBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + + ///base + num m_links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + if(bod->isMultiDof()) + { + if(!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); + else + { + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } + } + else + bod->stepPositions(timeStep); + + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + + if (bod->getBaseCollider()) + { + 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])); + + bod->getBaseCollider()->setWorldTransform(tr); + + } + + 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++) + { + btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(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])); + + col->setWorldTransform(tr); + } + } + } else + { + bod->clearVelocities(); + } + } + } +} + + + +void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.push_back(constraint); +} + +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.remove(constraint); +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index a4d1a1161..79b7758b1 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -1,56 +1,58 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H -#define BT_MULTIBODY_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" - - -class btMultiBody; -class btMultiBodyConstraint; -class btMultiBodyConstraintSolver; -struct MultiBodyInplaceSolverIslandCallback; - -///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet -///This implementation is still preliminary/experimental. -class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld -{ -protected: - btAlignedObjectArray m_multiBodies; - btAlignedObjectArray m_multiBodyConstraints; - btAlignedObjectArray m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); - virtual void integrateTransforms(btScalar timeStep); -public: - - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btMultiBodyDynamicsWorld (); - - virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); - - virtual void removeMultiBody(btMultiBody* body); - - virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); - - virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); -}; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H +#define BT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" + + +class btMultiBody; +class btMultiBodyConstraint; +class btMultiBodyConstraintSolver; +struct MultiBodyInplaceSolverIslandCallback; + +///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet +///This implementation is still preliminary/experimental. +class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld +{ +protected: + btAlignedObjectArray m_multiBodies; + btAlignedObjectArray m_multiBodyConstraints; + btAlignedObjectArray m_sortedMultiBodyConstraints; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); + +public: + + btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btMultiBodyDynamicsWorld (); + + virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); + + virtual void removeMultiBody(btMultiBody* body); + + virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + + virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); + + virtual void integrateTransforms(btScalar timeStep); +}; +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index b9658731b..778594b40 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -1,145 +1,145 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointLimitConstraint.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - -btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) - //:btMultiBodyConstraint(body,0,link,-1,2,true), - :btMultiBodyConstraint(body,body,link,link,2,true), - m_lowerBound(lower), - m_upperBound(upper) -{ - // the data.m_jacobians never change, so may as well - // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); - - // row 0: the lower bound - jacobianA(0)[offset] = 1; - // row 1: the upper bound - //jacobianA(1)[offset] = -1; - - jacobianB(1)[offset] = -1; -} -btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() -{ -} - -int btMultiBodyJointLimitConstraint::getIslandIdA() const -{ - if(m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyJointLimitConstraint::getIslandIdB() const -{ - if(m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - } - return -1; -} - - -void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent - - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = penetrationImpulse; - } - } - } - -} - - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointLimitConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) + //:btMultiBodyConstraint(body,0,link,-1,2,true), + :btMultiBodyConstraint(body,body,link,link,2,true), + m_lowerBound(lower), + m_upperBound(upper) +{ + // the data.m_jacobians never change, so may as well + // initialize them here + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + + unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); + + // row 0: the lower bound + jacobianA(0)[offset] = 1; + // row 1: the upper bound + //jacobianA(1)[offset] = -1; + + jacobianB(1)[offset] = -1; +} +btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() +{ +} + +int btMultiBodyJointLimitConstraint::getIslandIdA() const +{ + if(m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyJointLimitConstraint::getIslandIdB() const +{ + if(m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + +void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent + + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); + + for (int row=0;row infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + } + } + +} + + + + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 563ef4b2e..0c7fc1708 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -1,44 +1,44 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H -#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" -struct btSolverInfo; - -class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint -{ -protected: - - btScalar m_lowerBound; - btScalar m_upperBound; -public: - - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); - virtual ~btMultiBodyJointLimitConstraint(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - -}; - -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H +#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint +{ +protected: + + btScalar m_lowerBound; + btScalar m_upperBound; +public: + + btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); + virtual ~btMultiBodyJointLimitConstraint(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 18f49533b..29d505ebe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -110,7 +110,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - btScalar penetration = 0; + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 9e90c6f48..a793f510e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -25,7 +25,7 @@ enum btMultiBodyLinkFlags BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 }; -//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS +#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS #define TEST_SPATIAL_ALGEBRA_LAYER // @@ -368,7 +368,9 @@ struct btMultibodyLink // revolute: vector from parent's COM to the pivot point, in PARENT's frame. btVector3 m_eVector; +#ifdef TEST_SPATIAL_ALGEBRA_LAYER btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; +#endif enum eFeatherstoneJointType { @@ -378,11 +380,11 @@ struct btMultibodyLink #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS ePlanar = 3, #endif + eFixed = 4, eInvalid }; - eFeatherstoneJointType m_jointType; - int m_dofCount, m_posVarCount; //redundant but handy + // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. // for prismatic: m_axesTop[0] = zero; @@ -429,6 +431,10 @@ struct btMultibodyLink class btMultiBodyLinkCollider* m_collider; int m_flags; + + + int m_dofCount, m_posVarCount; //redundant but handy + eFeatherstoneJointType m_jointType; // ctor: set some sensible defaults btMultibodyLink() @@ -502,11 +508,18 @@ struct btMultibodyLink case ePlanar: { m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector); + m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); break; } #endif + case eFixed: + { + m_cachedRotParentToThis = m_zeroRotParentToThis; + m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } default: { //invalid type diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index f5622e062..5080ea874 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -1,92 +1,92 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H -#define BT_FEATHERSTONE_LINK_COLLIDER_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -#include "btMultiBody.h" - -class btMultiBodyLinkCollider : public btCollisionObject -{ -//protected: -public: - - btMultiBody* m_multiBody; - int m_link; - - - btMultiBodyLinkCollider (btMultiBody* multiBody,int link) - :m_multiBody(multiBody), - m_link(link) - { - m_checkCollideWith = true; - //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands - //this means that some constraints might point to bodies that are not in the islands, causing crashes - //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - } - // else - //{ - // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); - //} - - m_internalType = CO_FEATHERSTONE_LINK; - } - static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const - { - const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); - if (!other) - return true; - if (other->m_multiBody != this->m_multiBody) - return true; - if (!m_multiBody->hasSelfCollision()) - return false; - - //check if 'link' has collision disabled - if (m_link>=0) - { - const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) - return false; - } - - if (other->m_link>=0) - { - const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) - return false; - } - return true; - } -}; - -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H +#define BT_FEATHERSTONE_LINK_COLLIDER_H + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +#include "btMultiBody.h" + +class btMultiBodyLinkCollider : public btCollisionObject +{ +//protected: +public: + + btMultiBody* m_multiBody; + int m_link; + + + btMultiBodyLinkCollider (btMultiBody* multiBody,int link) + :m_multiBody(multiBody), + m_link(link) + { + m_checkCollideWith = true; + //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands + //this means that some constraints might point to bodies that are not in the islands, causing crashes + //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) + { + m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); + } + // else + //{ + // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); + //} + + m_internalType = CO_FEATHERSTONE_LINK; + } + static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); + if (!other) + return true; + if (other->m_multiBody != this->m_multiBody) + return true; + if (!m_multiBody->hasSelfCollision()) + return false; + + //check if 'link' has collision disabled + if (m_link>=0) + { + const btMultibodyLink& link = m_multiBody->getLink(this->m_link); + if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) + return false; + } + + if (other->m_link>=0) + { + const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); + if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) + return false; + } + return true; + } +}; + +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index d2ebf29ef..d7dbf5230 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -119,7 +119,6 @@ int numDim = BTMBP2PCONSTRAINT_DIM; contactNormalOnB[i%3] = -1; #endif - btScalar penetration = 0; // Convert local points back to world btVector3 pivotAworld = m_pivotInA; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index c9bfbc176..1295757a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -1,62 +1,62 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_POINT2POINT_H -#define BT_MULTIBODY_POINT2POINT_H - -#include "btMultiBodyConstraint.h" - -//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - -class btMultiBodyPoint2Point : public btMultiBodyConstraint -{ -protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - -public: - - btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); - btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); - - virtual ~btMultiBodyPoint2Point(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - -}; - -#endif //BT_MULTIBODY_POINT2POINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_POINT2POINT_H +#define BT_MULTIBODY_POINT2POINT_H + +#include "btMultiBodyConstraint.h" + +//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + +class btMultiBodyPoint2Point : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + +public: + + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + + virtual ~btMultiBodyPoint2Point(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + +}; + +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 45e31a176..7bc64569a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -1,85 +1,85 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H -#define BT_MULTIBODY_SOLVER_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btMultiBody; -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1) - {} - - int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - int m_jacAindex; - int m_deltaVelBindex; - int m_jacBindex; - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; - - btScalar m_friction; - btScalar m_jacDiagABInv; - btScalar m_rhs; - btScalar m_cfm; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_rhsPenetration; - union - { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - }; - - int m_overrideNumSolverIterations; - int m_frictionIndex; - - int m_solverBodyIdA; - btMultiBody* m_multiBodyA; - int m_linkA; - - int m_solverBodyIdB; - btMultiBody* m_multiBodyB; - int m_linkB; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray btMultiBodyConstraintArray; - -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H +#define BT_MULTIBODY_SOLVER_CONSTRAINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btMultiBody; +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1) + {} + + int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 + int m_jacAindex; + int m_deltaVelBindex; + int m_jacBindex; + + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + + int m_solverBodyIdA; + btMultiBody* m_multiBodyA; + int m_linkA; + + int m_solverBodyIdB; + btMultiBody* m_multiBodyB; + int m_linkB; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + +typedef btAlignedObjectArray btMultiBodyConstraintArray; + +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 3bf7b5c13..c2634dff5 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) /* declare variables - Z matrix, p and q vectors, etc */ btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; const btScalar *ell; - int lskip2,lskip3,i,j; + int lskip2,i,j; +// int lskip3; /* special handling for L and B because we're solving L1 *transpose* */ L = L + (n-1)*(lskip1+1); B = B + n-1; lskip1 = -lskip1; /* compute lskip values */ lskip2 = 2*lskip1; - lskip3 = 3*lskip1; + //lskip3 = 3*lskip1; /* compute all 4 x 1 blocks of X */ for (i=0; i <= n-4; i+=4) { /* compute all 4 x 1 block of X, from rows i..i+4-1 */ diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h index c9ff8a9f1..2a2f2d3d3 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -1,112 +1,112 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_DANTZIG_SOLVER_H -#define BT_DANTZIG_SOLVER_H - -#include "btMLCPSolverInterface.h" -#include "btDantzigLCP.h" - - -class btDantzigSolver : public btMLCPSolverInterface -{ -protected: - - btScalar m_acceptableUpperLimitSolution; - - btAlignedObjectArray m_tempBuffer; - - btAlignedObjectArray m_A; - btAlignedObjectArray m_b; - btAlignedObjectArray m_x; - btAlignedObjectArray m_lo; - btAlignedObjectArray m_hi; - btAlignedObjectArray m_dependencies; - btDantzigScratchMemory m_scratchMemory; -public: - - btDantzigSolver() - :m_acceptableUpperLimitSolution(btScalar(1000)) - { - } - - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) - { - bool result = true; - int n = b.rows(); - if (n) - { - int nub = 0; - btAlignedObjectArray ww; - ww.resize(n); - - - const btScalar* Aptr = A.getBufferPointer(); - m_A.resize(n*n); - for (int i=0;i= m_acceptableUpperLimitSolution) - { - return false; - } - - if (x[i] <= -m_acceptableUpperLimitSolution) - { - return false; - } - } - - for (int i=0;i m_tempBuffer; + + btAlignedObjectArray m_A; + btAlignedObjectArray m_b; + btAlignedObjectArray m_x; + btAlignedObjectArray m_lo; + btAlignedObjectArray m_hi; + btAlignedObjectArray m_dependencies; + btDantzigScratchMemory m_scratchMemory; +public: + + btDantzigSolver() + :m_acceptableUpperLimitSolution(btScalar(1000)) + { + } + + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) + { + bool result = true; + int n = b.rows(); + if (n) + { + int nub = 0; + btAlignedObjectArray ww; + ww.resize(n); + + + const btScalar* Aptr = A.getBufferPointer(); + m_A.resize(n*n); + for (int i=0;i= m_acceptableUpperLimitSolution) + { + return false; + } + + if (x[i] <= -m_acceptableUpperLimitSolution) + { + return false; + } + } + + for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) - { - - if (m_useLoHighBounds) - { - - BT_PROFILE("btLemkeSolver::solveMLCP"); - int n = A.rows(); - if (0==n) - return true; - - bool fail = false; - - btVectorXu solution(n); - btVectorXu q1; - q1.resize(n); - for (int row=0;rowm_maxValue) - { - if (x[i]> errorValueMax) - { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; - } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i]m_maxValue) - { - if (x[i]> errorValueMax) - { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; - } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i]& limitDependency, int numIterations, bool useSparsity = true) + { + + if (m_useLoHighBounds) + { + + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0==n) + return true; + + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row=0;rowm_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i] limitDependenciesCopy = m_limitDependencies; -// printf("solve first LCP\n"); - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); - if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); - - } else - { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); - } - return result; -} - -struct btJointNode -{ - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex;//-1 for null - int constraintRowIndex; -}; - - - -void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) -{ - int numContactRows = interleaveContactAndFriction ? 3 : 1; - - int numConstraintRows = m_allConstraintPtrArray.size(); - int n = numConstraintRows; - { - BT_PROFILE("init b (rhs)"); - m_b.resize(numConstraintRows); - m_bSplit.resize(numConstraintRows); - m_b.setZero(); - m_bSplit.setZero(); - for (int i=0;im_jacDiagABInv; - if (!btFuzzyZero(jacDiag)) - { - btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; - btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i]=rhs/jacDiag; - m_bSplit[i] = rhsPenetration/jacDiag; - } - - } - } - - btScalar* w = 0; - int nub = 0; - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - { - BT_PROFILE("init lo/ho"); - - for (int i=0;i=0) - { - m_lo[i] = -BT_INFINITY; - m_hi[i] = BT_INFINITY; - } else - { - m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; - m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - } - } - } - - // - int m=m_allConstraintPtrArray.size(); - - int numBodies = m_tmpSolverBodyPool.size(); - btAlignedObjectArray bodyJointNodeArray; - { - BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies,-1); - } - btAlignedObjectArray jointNodeArray; - { - BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); - } - - static btMatrixXu J3; - { - BT_PROFILE("J3.resize"); - J3.resize(2*m,8); - } - static btMatrixXu JinvM3; - { - BT_PROFILE("JinvM3.resize/setZero"); - - JinvM3.resize(2*m,8); - JinvM3.setZero(); - J3.setZero(); - } - int cur=0; - int rowOffset = 0; - static btAlignedObjectArray ofs; - { - BT_PROFILE("ofs resize"); - ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } - { - BT_PROFILE("Compute J and JinvM"); - int c=0; - - int numRows = 0; - - for (int i=0;im_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = im_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMass[r]); - JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); - } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); - } - } else - { - cur += numRows; - } - if (orgBodyB) - { - - { - int slotB=-1; - //find free jointNode slot for sbA - slotB =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); - int prevSlot = bodyJointNodeArray[sbB]; - bodyJointNodeArray[sbB] = slotB; - jointNodeArray[slotB].nextJointNodeIndex = prevSlot; - jointNodeArray[slotB].jointIndex = c; - jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; - jointNodeArray[slotB].constraintRowIndex = i; - } - - for (int row=0;rowm_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMassB[r]); - JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); - } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); - } - } - else - { - cur += numRows; - } - rowOffset+=numRows; - - } - - } - - - //compute JinvM = J*invM. - const btScalar* JinvM = JinvM3.getBufferPointer(); - - const btScalar* Jptr = J3.getBufferPointer(); - { - BT_PROFILE("m_A.resize"); - m_A.resize(n,n); - } - - { - BT_PROFILE("m_A.setZero"); - m_A.setZero(); - } - int c=0; - { - int numRows = 0; - BT_PROFILE("Compute A"); - for (int i=0;im_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = i=0) - { - int j0 = jointNodeArray[startJointNodeA].jointIndex; - int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; - //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r ( JinvMrow, - Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); - } - startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; - } - } - - { - int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB>=0) - { - int j1 = jointNodeArray[startJointNodeB].jointIndex; - int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - - if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; - m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, - Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); - } - startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; - } - } - } - - { - BT_PROFILE("compute diagonal"); - // compute diagonal blocks of m_A - - int row__ = 0; - int numJointRows = m_allConstraintPtrArray.size(); - - int jj=0; - for (;row__m_solverBodyIdA; - int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; - const btScalar *Jrow = Jptr + 2*8*(size_t)row__; - m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); - if (orgBodyB) - { - m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); - } - row__ += infom; - jj++; - } - } - } - - if (1) - { - // add cfm to the diagonal of m_A - for ( int i=0; im_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintPtrArray.size(); - - m_b.resize(numConstraintRows); - if (infoGlobal.m_splitImpulse) - m_bSplit.resize(numConstraintRows); - - m_bSplit.setZero(); - m_b.setZero(); - - for (int i=0;im_jacDiagABInv) - { - m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; - if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; - } - } - - static btMatrixXu Minv; - Minv.resize(6*numBodies,6*numBodies); - Minv.setZero(); - for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); - } - - static btMatrixXu J; - J.resize(numConstraintRows,6*numBodies); - J.setZero(); - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - for (int i=0;im_lowerLimit; - m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - - int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; - int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; - if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) - { - setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); - } - if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) - { - setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); - } - } - - static btMatrixXu J_transpose; - J_transpose= J.transpose(); - - static btMatrixXu tmp; - - { - { - BT_PROFILE("J*Minv"); - tmp = J*Minv; - - } - { - BT_PROFILE("J*tmp"); - m_A = tmp*J_transpose; - } - } - - if (1) - { - // add cfm to the diagonal of m_A - for ( int i=0; i limitDependenciesCopy = m_limitDependencies; +// printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); + + } else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + } + return result; +} + +struct btJointNode +{ + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex;//-1 for null + int constraintRowIndex; +}; + + + +void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction ? 3 : 1; + + int numConstraintRows = m_allConstraintPtrArray.size(); + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + m_b.setZero(); + m_bSplit.setZero(); + for (int i=0;im_jacDiagABInv; + if (!btFuzzyZero(jacDiag)) + { + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; + m_b[i]=rhs/jacDiag; + m_bSplit[i] = rhsPenetration/jacDiag; + } + + } + } + +// btScalar* w = 0; +// int nub = 0; + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + { + BT_PROFILE("init lo/ho"); + + for (int i=0;i=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } else + { + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + } + } + } + + // + int m=m_allConstraintPtrArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies,-1); + } + btAlignedObjectArray jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + } + + static btMatrixXu J3; + { + BT_PROFILE("J3.resize"); + J3.resize(2*m,8); + } + static btMatrixXu JinvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2*m,8); + JinvM3.setZero(); + J3.setZero(); + } + int cur=0; + int rowOffset = 0; + static btAlignedObjectArray ofs; + { + BT_PROFILE("ofs resize"); + ofs.resize(0); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); + } + { + BT_PROFILE("Compute J and JinvM"); + int c=0; + + int numRows = 0; + + for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = im_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + + for (int r=0;r<3;r++) + { + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur,r,normalInvMass[r]); + JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } else + { + cur += numRows; + } + if (orgBodyB) + { + + { + int slotB=-1; + //find free jointNode slot for sbA + slotB =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + for (int row=0;rowm_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + + for (int r=0;r<3;r++) + { + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur,r,normalInvMassB[r]); + JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } + else + { + cur += numRows; + } + rowOffset+=numRows; + + } + + } + + + //compute JinvM = J*invM. + const btScalar* JinvM = JinvM3.getBufferPointer(); + + const btScalar* Jptr = J3.getBufferPointer(); + { + BT_PROFILE("m_A.resize"); + m_A.resize(n,n); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c=0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i=0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + if (j0m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + //printf("%d joint i %d and j0: %d: ",count++,i,j0); + m_A.multiplyAdd2_p8r ( JinvMrow, + Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB>=0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; + m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, + Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintPtrArray.size(); + + int jj=0; + for (;row__m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; + + const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + const btScalar *Jrow = Jptr + 2*8*(size_t)row__; + m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); + if (orgBodyB) + { + m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + } + row__ += infom; + jj++; + } + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; im_tmpSolverBodyPool.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); + + m_b.resize(numConstraintRows); + if (infoGlobal.m_splitImpulse) + m_bSplit.resize(numConstraintRows); + + m_bSplit.setZero(); + m_b.setZero(); + + for (int i=0;im_jacDiagABInv) + { + m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; + if (infoGlobal.m_splitImpulse) + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; + } + } + + static btMatrixXu Minv; + Minv.resize(6*numBodies,6*numBodies); + Minv.setZero(); + for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); + } + + static btMatrixXu J; + J.resize(numConstraintRows,6*numBodies); + J.setZero(); + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + for (int i=0;im_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; + if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) + { + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); + } + if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) + { + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); + } + } + + static btMatrixXu J_transpose; + J_transpose= J.transpose(); + + static btMatrixXu tmp; + + { + { + BT_PROFILE("J*Minv"); + tmp = J*Minv; + + } + { + BT_PROFILE("J*tmp"); + m_A = tmp*J_transpose; + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; i m_limitDependencies; - btAlignedObjectArray m_allConstraintPtrArray; - btMLCPSolverInterface* m_solver; - int m_fallback; - btScalar m_cfm; - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - - virtual void createMLCP(const btContactSolverInfo& infoGlobal); - virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); - - //return true is it solves the problem successfully - virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); - -public: - - btMLCPSolver( btMLCPSolverInterface* solver); - virtual ~btMLCPSolver(); - - void setMLCPSolver(btMLCPSolverInterface* solver) - { - m_solver = solver; - } - - int getNumFallbacks() const - { - return m_fallback; - } - void setNumFallbacks(int num) - { - m_fallback = num; - } - - btScalar getCfm() const - { - return m_cfm; - } - void setCfm(btScalar cfm) - { - m_cfm = cfm; - } - - virtual btConstraintSolverType getSolverType() const - { - return BT_MLCP_SOLVER; - } - -}; - - -#endif //BT_MLCP_SOLVER_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_MLCP_SOLVER_H +#define BT_MLCP_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "LinearMath/btMatrixX.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" + +class btMLCPSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMatrixXu m_A; + btVectorXu m_b; + btVectorXu m_x; + btVectorXu m_lo; + btVectorXu m_hi; + + ///when using 'split impulse' we solve two separate (M)LCPs + btVectorXu m_bSplit; + btVectorXu m_xSplit; + btVectorXu m_bSplit1; + btVectorXu m_xSplit2; + + btAlignedObjectArray m_limitDependencies; + btAlignedObjectArray m_allConstraintPtrArray; + btMLCPSolverInterface* m_solver; + int m_fallback; + btScalar m_cfm; + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + + virtual void createMLCP(const btContactSolverInfo& infoGlobal); + virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); + + //return true is it solves the problem successfully + virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); + +public: + + btMLCPSolver( btMLCPSolverInterface* solver); + virtual ~btMLCPSolver(); + + void setMLCPSolver(btMLCPSolverInterface* solver) + { + m_solver = solver; + } + + int getNumFallbacks() const + { + return m_fallback; + } + void setNumFallbacks(int num) + { + m_fallback = num; + } + + btScalar getCfm() const + { + return m_cfm; + } + void setCfm(btScalar cfm) + { + m_cfm = cfm; + } + + virtual btConstraintSolverType getSolverType() const + { + return BT_MLCP_SOLVER; + } + +}; + + +#endif //BT_MLCP_SOLVER_H diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h index 705ff47b5..25bb3f6d3 100644 --- a/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h +++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -1,33 +1,33 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_MLCP_SOLVER_INTERFACE_H -#define BT_MLCP_SOLVER_INTERFACE_H - -#include "LinearMath/btMatrixX.h" - -class btMLCPSolverInterface -{ -public: - virtual ~btMLCPSolverInterface() - { - } - - //return true is it solves the problem successfully - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true)=0; -}; - -#endif //BT_MLCP_SOLVER_INTERFACE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_MLCP_SOLVER_INTERFACE_H +#define BT_MLCP_SOLVER_INTERFACE_H + +#include "LinearMath/btMatrixX.h" + +class btMLCPSolverInterface +{ +public: + virtual ~btMLCPSolverInterface() + { + } + + //return true is it solves the problem successfully + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true)=0; +}; + +#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h index ef045339a..9ec31a6d4 100644 --- a/src/BulletDynamics/MLCPSolvers/btPATHSolver.h +++ b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -1,151 +1,151 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - - -#ifndef BT_PATH_SOLVER_H -#define BT_PATH_SOLVER_H - -//#define BT_USE_PATH -#ifdef BT_USE_PATH - -extern "C" { -#include "PATH/SimpleLCP.h" -#include "PATH/License.h" -#include "PATH/Error_Interface.h" -}; - void __stdcall MyError(Void *data, Char *msg) -{ - printf("Path Error: %s\n",msg); -} - void __stdcall MyWarning(Void *data, Char *msg) -{ - printf("Path Warning: %s\n",msg); -} - -Error_Interface e; - - - -#include "btMLCPSolverInterface.h" -#include "Dantzig/lcp.h" - -class btPathSolver : public btMLCPSolverInterface -{ -public: - - btPathSolver() - { - License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0"); - e.error_data = 0; - e.warning = MyWarning; - e.error = MyError; - Error_SetInterface(&e); - } - - - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) - { - MCP_Termination status; - - - int numVariables = b.rows(); - if (0==numVariables) - return true; - - /* - variables - the number of variables in the problem - - m_nnz - the number of nonzeros in the M matrix - - m_i - a vector of size m_nnz containing the row indices for M - - m_j - a vector of size m_nnz containing the column indices for M - - m_ij - a vector of size m_nnz containing the data for M - - q - a vector of size variables - - lb - a vector of size variables containing the lower bounds on x - - ub - a vector of size variables containing the upper bounds on x - */ - btAlignedObjectArray values; - btAlignedObjectArray rowIndices; - btAlignedObjectArray colIndices; - - for (int i=0;i zResult; - zResult.resize(numVariables); - btAlignedObjectArray rhs; - btAlignedObjectArray upperBounds; - btAlignedObjectArray lowerBounds; - for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) + { + MCP_Termination status; + + + int numVariables = b.rows(); + if (0==numVariables) + return true; + + /* - variables - the number of variables in the problem + - m_nnz - the number of nonzeros in the M matrix + - m_i - a vector of size m_nnz containing the row indices for M + - m_j - a vector of size m_nnz containing the column indices for M + - m_ij - a vector of size m_nnz containing the data for M + - q - a vector of size variables + - lb - a vector of size variables containing the lower bounds on x + - ub - a vector of size variables containing the upper bounds on x + */ + btAlignedObjectArray values; + btAlignedObjectArray rowIndices; + btAlignedObjectArray colIndices; + + for (int i=0;i zResult; + zResult.resize(numVariables); + btAlignedObjectArray rhs; + btAlignedObjectArray upperBounds; + btAlignedObjectArray lowerBounds; + for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) - { - if (!A.rows()) - return true; - //the A matrix is sparse, so compute the non-zero elements - A.rowComputeNonZeroElements(); - - //A is a m-n matrix, m rows, n columns - btAssert(A.rows() == b.rows()); - - int i, j, numRows = A.rows(); - - float delta; - - for (int k = 0; k =0) - { - s = x[limitDependency[i]]; - if (s<0) - s=1; - } - - if (x[i]hi[i]*s) - x[i]=hi[i]*s; - } - } - return true; - } - -}; - -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H + + +#include "btMLCPSolverInterface.h" + +///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) +class btSolveProjectedGaussSeidel : public btMLCPSolverInterface +{ +public: + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) + { + if (!A.rows()) + return true; + //the A matrix is sparse, so compute the non-zero elements + A.rowComputeNonZeroElements(); + + //A is a m-n matrix, m rows, n columns + btAssert(A.rows() == b.rows()); + + int i, j, numRows = A.rows(); + + float delta; + + for (int k = 0; k =0) + { + s = x[limitDependency[i]]; + if (s<0) + s=1; + } + + if (x[i]hi[i]*s) + x[i]=hi[i]*s; + } + } + return true; + } + +}; + +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 77b475b96..a7b168846 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) int i=0; for (i=0;i& originalPoints, btAlignedObjectArray& hull, const btVector3& normalAxis) -{ - btVector3 axis0,axis1; - btPlaneSpace1(normalAxis,axis0,axis1); - - - if (originalPoints.size()<=1) - { - for (int i=0;i1) { - btVector3& a = hull[hull.size()-2]; - btVector3& b = hull[hull.size()-1]; - isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0; - if (!isConvex) - hull.pop_back(); - else - hull.push_back(originalPoints[i]); - } - } -} - -#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H +#define GRAHAM_SCAN_2D_CONVEX_HULL_H + + +#include "btVector3.h" +#include "btAlignedObjectArray.h" + +struct GrahamVector3 : public btVector3 +{ + GrahamVector3(const btVector3& org, int orgIndex) + :btVector3(org), + m_orgIndex(orgIndex) + { + } + btScalar m_angle; + int m_orgIndex; +}; + + +struct btAngleCompareFunc { + btVector3 m_anchor; + btAngleCompareFunc(const btVector3& anchor) + : m_anchor(anchor) + { + } + bool operator()(const GrahamVector3& a, const GrahamVector3& b) const { + if (a.m_angle != b.m_angle) + return a.m_angle < b.m_angle; + else + { + btScalar al = (a-m_anchor).length2(); + btScalar bl = (b-m_anchor).length2(); + if (al != bl) + return al < bl; + else + { + return a.m_orgIndex < b.m_orgIndex; + } + } + } +}; + +inline void GrahamScanConvexHull2D(btAlignedObjectArray& originalPoints, btAlignedObjectArray& hull, const btVector3& normalAxis) +{ + btVector3 axis0,axis1; + btPlaneSpace1(normalAxis,axis0,axis1); + + + if (originalPoints.size()<=1) + { + for (int i=0;i1) { + btVector3& a = hull[hull.size()-2]; + btVector3& b = hull[hull.size()-1]; + isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0; + if (!isConvex) + hull.pop_back(); + else + hull.push_back(originalPoints[i]); + } + } +} + +#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index c76df7554..a54b3d404 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -46,7 +46,8 @@ class btIDebugDraw DBG_DrawConstraints = (1 << 11), DBG_DrawConstraintLimits = (1 << 12), DBG_FastWireframe = (1<<13), - DBG_DrawNormals = (1<<14), + DBG_DrawNormals = (1<<14), + DBG_DrawFrames = (1<<15), DBG_MAX_DEBUG_DRAW_MODE }; @@ -147,7 +148,7 @@ class btIDebugDraw const btVector3& vx = axis; btVector3 vy = normal.cross(axis); btScalar step = stepDegrees * SIMD_RADS_PER_DEG; - int nSteps = (int)((maxAngle - minAngle) / step); + int nSteps = (int)btFabs((maxAngle - minAngle) / step); if(!nSteps) nSteps = 1; btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); if(drawSect) @@ -438,11 +439,10 @@ class btIDebugDraw drawLine(transform*pt0,transform*pt1,color); drawLine(transform*pt2,transform*pt3,color); } - - virtual void flushLines() - { - - } + + virtual void flushLines() + { + } }; diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h index b025da7a1..abaaf6fd7 100644 --- a/src/LinearMath/btMatrixX.h +++ b/src/LinearMath/btMatrixX.h @@ -1,553 +1,553 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_MATRIX_X_H -#define BT_MATRIX_X_H - -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btAlignedObjectArray.h" - -//#define BT_DEBUG_OSTREAM -#ifdef BT_DEBUG_OSTREAM -#include -#include // std::setw -#endif //BT_DEBUG_OSTREAM - -class btIntSortPredicate -{ - public: - bool operator() ( const int& a, const int& b ) const - { - return a < b; - } -}; - - -template -struct btVectorX -{ - btAlignedObjectArray m_storage; - - btVectorX() - { - } - btVectorX(int numRows) - { - m_storage.resize(numRows); - } - - void resize(int rows) - { - m_storage.resize(rows); - } - int cols() const - { - return 1; - } - int rows() const - { - return m_storage.size(); - } - int size() const - { - return rows(); - } - - T nrm2() const - { - T norm = T(0); - - int nn = rows(); - - { - if (nn == 1) - { - norm = btFabs((*this)[0]); - } - else - { - T scale = 0.0; - T ssq = 1.0; - - /* The following loop is equivalent to this call to the LAPACK - auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ - - for (int ix=0;ix - void setElem(btMatrixX& mat, int row, int col, T val) - { - mat.setElem(row,col,val); - } - */ - - -template -struct btMatrixX -{ - int m_rows; - int m_cols; - int m_operations; - int m_resizeOperations; - int m_setElemOperations; - - btAlignedObjectArray m_storage; - mutable btAlignedObjectArray< btAlignedObjectArray > m_rowNonZeroElements1; - - T* getBufferPointerWritable() - { - return m_storage.size() ? &m_storage[0] : 0; - } - - const T* getBufferPointer() const - { - return m_storage.size() ? &m_storage[0] : 0; - } - btMatrixX() - :m_rows(0), - m_cols(0), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) - { - } - btMatrixX(int rows,int cols) - :m_rows(rows), - m_cols(cols), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) - { - resize(rows,cols); - } - void resize(int rows, int cols) - { - m_resizeOperations++; - m_rows = rows; - m_cols = cols; - { - BT_PROFILE("m_storage.resize"); - m_storage.resize(rows*cols); - } - } - int cols() const - { - return m_cols; - } - int rows() const - { - return m_rows; - } - ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead - /*T& operator() (int row,int col) - { - return m_storage[col*m_rows+row]; - } - */ - - void addElem(int row,int col, T val) - { - if (val) - { - if (m_storage[col+row*m_cols]==0.f) - { - setElem(row,col,val); - } else - { - m_storage[row*m_cols+col] += val; - } - } - } - - - void setElem(int row,int col, T val) - { - m_setElemOperations++; - m_storage[row*m_cols+col] = val; - } - - void mulElem(int row,int col, T val) - { - m_setElemOperations++; - //mul doesn't change sparsity info - - m_storage[row*m_cols+col] *= val; - } - - - - - void copyLowerToUpperTriangle() - { - int count=0; - for (int row=0;row0 && numRowsOther>0 && B && C); - const btScalar *bb = B; - for ( int i = 0;i& block) - { - btAssert(rowend+1-rowstart == block.rows()); - btAssert(colend+1-colstart == block.cols()); - for (int row=0;row btMatrixXf; -typedef btVectorX btVectorXf; - -typedef btMatrixX btMatrixXd; -typedef btVectorX btVectorXd; - - -#ifdef BT_DEBUG_OSTREAM -template -std::ostream& operator<< (std::ostream& os, const btMatrixX& mat) - { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i -std::ostream& operator<< (std::ostream& os, const btVectorX& mat) - { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i +#include // std::setw +#endif //BT_DEBUG_OSTREAM + +class btIntSortPredicate +{ + public: + bool operator() ( const int& a, const int& b ) const + { + return a < b; + } +}; + + +template +struct btVectorX +{ + btAlignedObjectArray m_storage; + + btVectorX() + { + } + btVectorX(int numRows) + { + m_storage.resize(numRows); + } + + void resize(int rows) + { + m_storage.resize(rows); + } + int cols() const + { + return 1; + } + int rows() const + { + return m_storage.size(); + } + int size() const + { + return rows(); + } + + T nrm2() const + { + T norm = T(0); + + int nn = rows(); + + { + if (nn == 1) + { + norm = btFabs((*this)[0]); + } + else + { + T scale = 0.0; + T ssq = 1.0; + + /* The following loop is equivalent to this call to the LAPACK + auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ + + for (int ix=0;ix + void setElem(btMatrixX& mat, int row, int col, T val) + { + mat.setElem(row,col,val); + } + */ + + +template +struct btMatrixX +{ + int m_rows; + int m_cols; + int m_operations; + int m_resizeOperations; + int m_setElemOperations; + + btAlignedObjectArray m_storage; + mutable btAlignedObjectArray< btAlignedObjectArray > m_rowNonZeroElements1; + + T* getBufferPointerWritable() + { + return m_storage.size() ? &m_storage[0] : 0; + } + + const T* getBufferPointer() const + { + return m_storage.size() ? &m_storage[0] : 0; + } + btMatrixX() + :m_rows(0), + m_cols(0), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) + { + } + btMatrixX(int rows,int cols) + :m_rows(rows), + m_cols(cols), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) + { + resize(rows,cols); + } + void resize(int rows, int cols) + { + m_resizeOperations++; + m_rows = rows; + m_cols = cols; + { + BT_PROFILE("m_storage.resize"); + m_storage.resize(rows*cols); + } + } + int cols() const + { + return m_cols; + } + int rows() const + { + return m_rows; + } + ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead + /*T& operator() (int row,int col) + { + return m_storage[col*m_rows+row]; + } + */ + + void addElem(int row,int col, T val) + { + if (val) + { + if (m_storage[col+row*m_cols]==0.f) + { + setElem(row,col,val); + } else + { + m_storage[row*m_cols+col] += val; + } + } + } + + + void setElem(int row,int col, T val) + { + m_setElemOperations++; + m_storage[row*m_cols+col] = val; + } + + void mulElem(int row,int col, T val) + { + m_setElemOperations++; + //mul doesn't change sparsity info + + m_storage[row*m_cols+col] *= val; + } + + + + + void copyLowerToUpperTriangle() + { + int count=0; + for (int row=0;row0 && numRowsOther>0 && B && C); + const btScalar *bb = B; + for ( int i = 0;i& block) + { + btAssert(rowend+1-rowstart == block.rows()); + btAssert(colend+1-colstart == block.cols()); + for (int row=0;row btMatrixXf; +typedef btVectorX btVectorXf; + +typedef btMatrixX btMatrixXd; +typedef btVectorX btVectorXd; + + +#ifdef BT_DEBUG_OSTREAM +template +std::ostream& operator<< (std::ostream& os, const btMatrixX& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;i +std::ostream& operator<< (std::ostream& os, const btVectorX& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;i #endif -#if defined (SUNOS) || defined (__SUNOS__) -#include +#if defined (SUNOS) || defined (__SUNOS__) +#include #endif #if defined(WIN32) || defined(_WIN32) @@ -37,7 +37,7 @@ static btClock gProfileClock; #define WIN32_LEAN_AND_MEAN #define NOWINRES #define NOMCX -#define NOIME +#define NOIME #ifdef _XBOX #include @@ -121,34 +121,34 @@ void btClock::reset() #endif } -/// Returns the time in ms since the last call to reset or since +/// Returns the time in ms since the last call to reset or since /// the btClock was created. unsigned long int btClock::getTimeMilliseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + LONGLONG elapsedTime = currentTime.QuadPart - m_data->mStartTime.QuadPart; // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA // bridge, aka south bridge. See Microsoft KB274323.) unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; signed long msecOff = (signed long)(msecTicks - elapsedTicks); if (msecOff < -100 || msecOff > 100) { // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - m_data->mPrevElapsedTime); m_data->mStartTime.QuadPart += msecAdjustment; elapsedTime -= msecAdjustment; // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / + msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); } @@ -171,36 +171,36 @@ unsigned long int btClock::getTimeMilliseconds() struct timeval currentTime; gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000; #endif //__CELLOS_LV2__ #endif } - /// Returns the time in us since the last call to reset or since + /// Returns the time in us since the last call to reset or since /// the Clock was created. unsigned long int btClock::getTimeMicroseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + LONGLONG elapsedTime = currentTime.QuadPart - m_data->mStartTime.QuadPart; // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA // bridge, aka south bridge. See Microsoft KB274323.) unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; signed long msecOff = (signed long)(msecTicks - elapsedTicks); if (msecOff < -100 || msecOff > 100) { // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - + LONGLONG msecAdjustment = mymin(msecOff * + m_data->mClockFrequency.QuadPart / 1000, elapsedTime - m_data->mPrevElapsedTime); m_data->mStartTime.QuadPart += msecAdjustment; elapsedTime -= msecAdjustment; @@ -210,7 +210,7 @@ unsigned long int btClock::getTimeMicroseconds() m_data->mPrevElapsedTime = elapsedTime; // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / m_data->mClockFrequency.QuadPart); return usecTicks; @@ -229,10 +229,10 @@ unsigned long int btClock::getTimeMicroseconds() struct timeval currentTime; gettimeofday(¤tTime, 0); - return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + (currentTime.tv_usec - m_data->mStartTime.tv_usec); #endif//__CELLOS_LV2__ -#endif +#endif } @@ -325,7 +325,7 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) } // We didn't find it, so add it - + CProfileNode * node = new CProfileNode( name, this ); node->Sibling = Child; Child = node; @@ -337,7 +337,7 @@ void CProfileNode::Reset( void ) { TotalCalls = 0; TotalTime = 0.0f; - + if ( Child ) { Child->Reset(); @@ -359,7 +359,7 @@ void CProfileNode::Call( void ) bool CProfileNode::Return( void ) { - if ( --RecursionCounter == 0 && TotalCalls != 0 ) { + if ( --RecursionCounter == 0 && TotalCalls != 0 ) { unsigned long int time; Profile_Get_Ticks(&time); time-=StartTime; @@ -452,8 +452,8 @@ void CProfileManager::Start_Profile( const char * name ) { if (name != CurrentNode->Get_Name()) { CurrentNode = CurrentNode->Get_Sub_Node( name ); - } - + } + CurrentNode->Call(); } @@ -477,7 +477,7 @@ void CProfileManager::Stop_Profile( void ) * This resets everything except for the tree structure. All of the timing data is reset. * *=============================================================================================*/ void CProfileManager::Reset( void ) -{ +{ gProfileClock.reset(); Root.Reset(); Root.Call(); @@ -523,9 +523,9 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time ); float totalTime = 0.f; - + int numChildren = 0; - + for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) { numChildren++; @@ -546,7 +546,7 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci } for (i=0;i SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); - + for (i=0;iEnter_Child(i); diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index bcf81ad58..3e54536c2 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -97,7 +97,7 @@ inline int btGetVersion() #ifdef BT_DEBUG #ifdef _MSC_VER #include - #define btAssert(x) { if(!(x)){printf("Assert " __FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} + #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} #else//_MSC_VER #include #define btAssert assert @@ -125,7 +125,7 @@ inline int btGetVersion() #ifdef __SPU__ #include #define printf spu_printf - #define btAssert(x) {if(!(x)){printf("Assert " __FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} + #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} #else #define btAssert assert #endif @@ -343,12 +343,23 @@ inline __m128 operator * (const __m128 A, const __m128 B) #else//BT_USE_NEON #ifndef BT_INFINITY - static int btInfinityMask = 0x7F800000; - #define BT_INFINITY (*(float*)&btInfinityMask) - inline int btGetInfinityMask()//suppress stupid compiler warning - { - return btInfinityMask; - } + struct btInfMaskConverter + { + union { + float mask; + int intmask; + }; + btInfMaskConverter(int mask=0x7F800000) + :intmask(mask) + { + } + }; + static btInfMaskConverter btInfinityMask = 0x7F800000; + #define BT_INFINITY (btInfinityMask.mask) + inline int btGetInfinityMask()//suppress stupid compiler warning + { + return btInfinityMask.mask; + } #endif #endif//BT_USE_NEON diff --git a/src/LinearMath/premake4.lua b/src/LinearMath/premake4.lua index b698bed8e..e4ba87b3e 100644 --- a/src/LinearMath/premake4.lua +++ b/src/LinearMath/premake4.lua @@ -1,11 +1,11 @@ - project "LinearMath" - - kind "StaticLib" - targetdir "../../lib" - includedirs { - "..", - } - files { - "**.cpp", - "**.h" - } \ No newline at end of file + project "LinearMath" + + kind "StaticLib" + targetdir "../../lib" + includedirs { + "..", + } + files { + "*.cpp", + "*.h" + } diff --git a/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp b/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp index 7756e7bb0..d45b7d294 100644 --- a/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp +++ b/test/OpenCL/AllBullet3Kernels/testExecuteBullet3NarrowphaseKernels.cpp @@ -21,7 +21,7 @@ extern char** gArgv; namespace { - struct ExecuteBullet3NarrowphaseKernels : public ::testing::Test + struct ExecuteBullet3NarrowphaseKernels : public ::testing::Test { cl_context m_clContext; cl_device_id m_clDevice; @@ -41,7 +41,7 @@ namespace initCL(); } - virtual ~ExecuteBullet3NarrowphaseKernels() + virtual ~ExecuteBullet3NarrowphaseKernels() { // You can do clean-up work that doesn't throw exceptions here. exitCL(); @@ -51,8 +51,8 @@ namespace // and cleaning up each test, you can define the following methods: #include "initCL.h" - - virtual void SetUp() + + virtual void SetUp() { @@ -60,7 +60,7 @@ namespace // before each test). } - virtual void TearDown() + virtual void TearDown() { // Code here will be called immediately after each test (right // before the destructor). @@ -71,13 +71,13 @@ namespace TEST_F(ExecuteBullet3NarrowphaseKernels,satKernelsCL) { cl_int errNum=0; - + char flags[1024]={0}; cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_clContext,m_clDevice,satKernelsCL,&errNum,flags,0,true); ASSERT_EQ(CL_SUCCESS,errNum); - + { cl_kernel m_findSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satKernelsCL, "findSeparatingAxisKernel",&errNum,satProg ); ASSERT_EQ(CL_SUCCESS,errNum); @@ -101,9 +101,9 @@ namespace ASSERT_EQ(CL_SUCCESS,errNum); clReleaseKernel(m_findConcaveSeparatingAxisKernel ); } - - - { + + + { cl_kernel m_findCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satKernelsCL, "findCompoundPairsKernel",&errNum,satProg ); ASSERT_EQ(CL_SUCCESS,errNum); clReleaseKernel(m_findCompoundPairsKernel); @@ -122,7 +122,7 @@ namespace TEST_F(ExecuteBullet3NarrowphaseKernels,satConcaveKernelsCL) { cl_int errNum=0; - + char flags[1024]={0}; cl_program satConcaveProg = b3OpenCLUtils::compileCLProgramFromString(m_clContext,m_clDevice,satConcaveKernelsCL,&errNum,flags,0,true); @@ -133,20 +133,20 @@ namespace ASSERT_EQ(CL_SUCCESS,errNum); clReleaseKernel(m_findConcaveSeparatingAxisVertexFaceKernel); } - + { cl_kernel m_findConcaveSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satConcaveKernelsCL, "findConcaveSeparatingAxisEdgeEdgeKernel",&errNum,satConcaveProg ); ASSERT_EQ(CL_SUCCESS,errNum); clReleaseKernel(m_findConcaveSeparatingAxisEdgeEdgeKernel); } - + clReleaseProgram(satConcaveProg); } TEST_F(ExecuteBullet3NarrowphaseKernels,satClipKernelsCL) { - + char flags[1024]={0}; cl_int errNum=0; //#ifdef CL_PLATFORM_INTEL @@ -167,7 +167,7 @@ namespace ASSERT_EQ(CL_SUCCESS,errNum); clReleaseKernel(m_clipCompoundsHullHullKernel); } - + { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satClipKernelsCL, "findClippingFacesKernel",&errNum,satClipContactsProg); ASSERT_EQ(CL_SUCCESS,errNum); @@ -177,13 +177,13 @@ namespace { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satClipKernelsCL, "clipFacesAndFindContactsKernel",&errNum,satClipContactsProg); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satClipKernelsCL, "clipHullHullConcaveConvexKernel",&errNum,satClipContactsProg); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } @@ -191,9 +191,9 @@ namespace cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,satClipKernelsCL, "newContactReductionKernel",&errNum,satClipContactsProg); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } - + clReleaseProgram(satClipContactsProg); } @@ -201,7 +201,7 @@ namespace TEST_F(ExecuteBullet3NarrowphaseKernels,bvhTraversalKernels) { - + cl_int errNum=0; cl_program bvhTraversalProg = b3OpenCLUtils::compileCLProgramFromString(m_clContext,m_clDevice,bvhTraversalKernelCL,&errNum,"",0,true); ASSERT_EQ(CL_SUCCESS,errNum); @@ -209,7 +209,7 @@ namespace { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,bvhTraversalKernelCL, "bvhTraversalKernel",&errNum,bvhTraversalProg,""); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } clReleaseProgram(bvhTraversalProg); } @@ -224,21 +224,21 @@ namespace { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,primitiveContactsKernelsCL, "primitiveContactsKernel",&errNum,primitiveContactsProg,""); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,primitiveContactsKernelsCL, "findConcaveSphereContactsKernel",&errNum,primitiveContactsProg ); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,primitiveContactsKernelsCL, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,""); ASSERT_EQ(CL_SUCCESS,errNum); - clReleaseKernel(k); + clReleaseKernel(k); } - + clReleaseProgram(primitiveContactsProg); } @@ -272,8 +272,8 @@ namespace unzFile zipfile = unzOpen( relativeFileName); if ( zipfile == NULL ) { - printf( "%s: not found\n" ); - + printf( "%s: not found\n" ,relativeFileName); + } // Get info about the zip file @@ -286,17 +286,17 @@ namespace } else { result = unzLocateFile(zipfile, fileName, 0); - if (result == UNZ_OK) + if (result == UNZ_OK) { unz_file_info info; result = unzGetCurrentFileInfo( zipfile, &info, NULL, 0, NULL, 0, NULL, 0 ); - if ( result != UNZ_OK ) + if ( result != UNZ_OK ) { b3Printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); } else { result = unzOpenCurrentFile(zipfile); - if (result == UNZ_OK) + if (result == UNZ_OK) { buffer = (unsigned char*)malloc(info.uncompressed_size); result = unzReadCurrentFile(zipfile,buffer,info.uncompressed_size); @@ -334,7 +334,7 @@ namespace f = fopen(relativeFileName,"rb"); } //first try from data.zip, otherwise directly load the file from disk - + if (f) { int sizeInBytes=0; @@ -355,7 +355,7 @@ namespace } fclose(f); } - + } return buffer; @@ -374,7 +374,7 @@ namespace int num = *(int*)&buf[serializedBytes]; launcher.launch1D( num); - + free(buf); //this clFinish is for testing on errors } @@ -382,13 +382,13 @@ namespace TEST_F(ExecuteBullet3NarrowphaseKernels,mprKernelsCL) { - + cl_int errNum=0; const char* srcConcave = satConcaveKernelsCL; char flags[1024]={0}; cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_clContext,m_clDevice,mprKernelsCL,&errNum,flags,0,true); ASSERT_EQ(CL_SUCCESS,errNum); - + { cl_kernel k = b3OpenCLUtils::compileCLKernelFromString(m_clContext, m_clDevice,mprKernelsCL, "mprPenetrationKernel",&errNum,mprProg ); ASSERT_EQ(CL_SUCCESS,errNum); @@ -397,7 +397,7 @@ namespace { const char* fileNames[]={"mprPenetrationKernel60.bin","mprPenetrationKernel61.bin","mprPenetrationKernel70.bin","mprPenetrationKernel128.bin"}; int results[] = {0,1,46,98}; - + int numTests = sizeof(fileNames)/sizeof(const char*); for (int i=0;i atomicCounter(context,queue); b3OpenCLArray deviceContacts(context,queue); b3AlignedObjectArray hostContacts; - + b3Contact4Data tmp; int sz = sizeof(b3Contact4Data); memset(&tmp,1,sz); @@ -182,7 +182,7 @@ int main(int argc, char* argv[]) memset(&tmp,2,sz); deviceContacts.push_back(tmp); b3Contact4Data tmp3 = tmp; - + atomicCounter.push_back(0); deviceElements.resize(numWorkItems); @@ -190,7 +190,7 @@ int main(int argc, char* argv[]) run.setBuffer(deviceElements.getBufferCL()); run.setBuffer(deviceContacts.getBufferCL()); run.setBuffer(atomicCounter.getBufferCL()); - + run.launch1D(numWorkItems); b3AlignedObjectArray hostElements; @@ -198,7 +198,7 @@ int main(int argc, char* argv[]) deviceContacts.copyToHost(hostContacts); tmp2 = hostContacts[0]; tmp3 = hostContacts[1]; - + printf("...\n"); @@ -207,7 +207,7 @@ int main(int argc, char* argv[]) printf("kernel failed to compile\n"); } - + } }