diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index de0b22c8d..a481dbf84 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -1011,6 +1011,37 @@ void btWorldImporter::convertConstraintFloat(btTypedConstraintFloatData* constra break; } + case FIXED_CONSTRAINT_TYPE: + { + + btGeneric6DofSpring2Constraint* dof = 0; + if (rbA && rbB) + { + btTransform rbAFrame,rbBFrame; + //compute a shared world frame, and compute frameInA, frameInB relative to this + btTransform sharedFrame; + sharedFrame.setIdentity(); + btVector3 centerPos = btScalar(0.5)*(rbA->getWorldTransform().getOrigin()+ + rbB->getWorldTransform().getOrigin()); + sharedFrame.setOrigin(centerPos); + rbAFrame = rbA->getWorldTransform().inverse()*sharedFrame; + rbBFrame = rbB->getWorldTransform().inverse()*sharedFrame; + + + dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, RO_XYZ); + dof->setLinearUpperLimit(btVector3(0,0,0)); + dof->setLinearLowerLimit(btVector3(0,0,0)); + dof->setAngularUpperLimit(btVector3(0,0,0)); + dof->setAngularLowerLimit(btVector3(0,0,0)); + + } else + { + printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n"); + } + + constraint = dof; + break; + } default: { printf("unknown constraint type\n"); @@ -1310,6 +1341,38 @@ void btWorldImporter::convertConstraintDouble(btTypedConstraintDoubleData* const break; } + case FIXED_CONSTRAINT_TYPE: + { + + btGeneric6DofSpring2Constraint* dof = 0; + if (rbA && rbB) + { + btTransform rbAFrame,rbBFrame; + //compute a shared world frame, and compute frameInA, frameInB relative to this + btTransform sharedFrame; + sharedFrame.setIdentity(); + btVector3 centerPos = btScalar(0.5)*(rbA->getWorldTransform().getOrigin()+ + rbB->getWorldTransform().getOrigin()); + sharedFrame.setOrigin(centerPos); + rbAFrame = rbA->getWorldTransform().inverse()*sharedFrame; + rbBFrame = rbB->getWorldTransform().inverse()*sharedFrame; + + + dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, RO_XYZ); + dof->setLinearUpperLimit(btVector3(0,0,0)); + dof->setLinearLowerLimit(btVector3(0,0,0)); + dof->setAngularUpperLimit(btVector3(0,0,0)); + dof->setAngularLowerLimit(btVector3(0,0,0)); + + } else + { + printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n"); + } + + constraint = dof; + break; + } + default: { printf("unknown constraint type\n"); diff --git a/docs/BulletQuickstart.pdf b/docs/BulletQuickstart.pdf index a78539c08..c6b491303 100644 Binary files a/docs/BulletQuickstart.pdf and b/docs/BulletQuickstart.pdf differ diff --git a/docs/Bullet_User_Manual.pdf b/docs/Bullet_User_Manual.pdf index e68a91698..8e980b666 100644 Binary files a/docs/Bullet_User_Manual.pdf and b/docs/Bullet_User_Manual.pdf differ diff --git a/docs/latex/building.tex b/docs/latex/building.tex index 49b19346d..73adcb265 100644 --- a/docs/latex/building.tex +++ b/docs/latex/building.tex @@ -2,13 +2,10 @@ Windows developers can download the zipped sources of Bullet from \url{http://bullet.googlecode.com}. Mac OS X, Linux and other developers should download the gzipped tar archive. Bullet provides several build systems. -\section{Microsoft Visual Studio} -After unzipping the source code you can open the \path{Bullet/build/vs2010/0BulletSolution.sln}, hit F5 and your first Bullet demo will run. Note that by default Visual Studio uses an unoptimized Debug configuration that is very slow. It is best to enable the Release configuration. - \section{Using Premake} \index{premake}\href{http://industriousone.com/premake}{Premake} is a meta build system based on the Lua scripting language that can generate project files for Microsoft Visual Studio, Apple Xcode as well as Makefiles for GNU make and other build systems. Bullet comes with Premake executables for Windows, Mac OSX and Linux. \subsection{Premake Visual Studio project generation} -You can double-click on \path{Bullet/build/vs2010.bat} to generate Visual Studio 2010 project files and solution. This batch file calls Premake. Just open \path{Bullet/build/vs2010/0BulletSolution.sln} +You can double-click on \path{Bullet/build/vs2010.bat} to generate Visual Studio 2010 project files and solution. This batch file calls Premake. Just open \path{Bullet/build/vs2010/0BulletSolution.sln}. Newer versions of Visual Studio should automatically convert from the 2010 solution. \subsection{Premake Mac OSX Xcode project generation} On Mac OSX it is easiest to open a Terminal window and switch current directory to Bullet/build and use the following command to generate XCode projects: \begin{lstlisting}[caption=Premake for Mac OSX, label=premake_osx] @@ -17,14 +14,6 @@ cd Bullet/build open xcode4/0BulletSolution.xcworkspace \end{lstlisting} -\subsection{Premake iOS Xcode project generation} -XCode project generation for iOS, such as iPhone and iPad, is similar to Mac OSX. Just provide the --ios option to premake and premake will automatically customize the project and append ios to the directory: - \begin{lstlisting}[caption=Premake for iOS, label=premake_ios] -cd Bullet/build -./premake_osx --ios xcode4 -open xcode4ios/0BulletSolution.xcworkspace -\end{lstlisting} -Note that Bullet comes with a modified version of \path{premake_osx} that enables the iOS specific customizations that are required by XCode. \subsection{Premake GNU Makefile generation} You can also generate GNU Makefiles for Mac OSX or Linux using premake: \begin{lstlisting}[caption=Premake to GNU Makefile, label=premake_make] @@ -34,12 +23,20 @@ cd gmake make config=release64 \end{lstlisting} \section{Using cmake} -Similar to premake, CMake adds support for many other build environments and platforms, including Microsoft Visual Studio, XCode for Mac OSX, KDevelop for Linux and Unix Makefiles. Download and install \index{CMake}CMake from \url{http://cmake.org} and use the CMake cmake-gui tool. -\section{Using autotools} -Open a shell terminal and go to the Bullet root directory. Then execute -\begin{lstlisting}[caption=autotools to Makefile, label=autotools_make] -./autogen.sh -./configure +Similar to premake, CMake adds support for many other build environments and platforms, including Microsoft Visual Studio, XCode for Mac OSX, KDevelop for Linux and Unix Makefiles. Download and install \index{CMake}CMake from \url{http://cmake.org} and use the CMake cmake-gui tool. Alternatively use a terminal, change to the Bullet root directory and use for example the following commands: + +\begin{lstlisting}[caption=CMake to GNU Makefile, label=cmake_make] +cmake . make \end{lstlisting} -The \path{autogen.sh} step is optional and not needed for downloaded packages. +An example to create an XCode project on Mac OSX using CMake: +\begin{lstlisting}[caption=CMake to Xcode, label=cmake_xcode] +mkdir xcode +cd xcode +cmake .. -G Xcode +open BULLET_PHYSICS.xcodeproj +\end{lstlisting} + + +\section{Executing the Example Browser} +After building the SDK, there are some binary executables in the bin folder. You can execute the $bin/App_ExampleBrowser_*$ to experiment with the Bullet Physics SDK. \ No newline at end of file diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp index a1ede8ff4..ca9f75786 100644 --- a/examples/Benchmarks/BenchmarkDemo.cpp +++ b/examples/Benchmarks/BenchmarkDemo.cpp @@ -105,6 +105,14 @@ class BenchmarkDemo : public CommonRigidBodyBase void stepSimulation(float deltaTime); + void resetCamera() + { + float dist = 120; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,10.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } }; @@ -118,7 +126,8 @@ public: btVector3 direction[NUMRAYS]; btVector3 hit[NUMRAYS]; btVector3 normal[NUMRAYS]; - + struct GUIHelperInterface* m_guiHelper; + int frame_counter; int ms; int sum_ms; @@ -138,6 +147,7 @@ public: btRaycastBar2 () { + m_guiHelper = 0; ms = 0; max_ms = 0; min_ms = 9999; @@ -147,8 +157,9 @@ public: - btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y) + btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y,struct GUIHelperInterface* guiHelper) { + m_guiHelper = guiHelper; frame_counter = 0; ms = 0; max_ms = 0; @@ -253,6 +264,33 @@ public: void draw () { + + if (m_guiHelper) + { + btAlignedObjectArray indices; + btAlignedObjectArray points; + + + float lineColor[4]={1,0.4,.4,1}; + + for (int i = 0; i < NUMRAYS; i++) + { + btVector3FloatData s,h; + for (int w=0;w<4;w++) + { + s.m_floats[w] = source[i][w]; + h.m_floats[w] = hit[i][w]; + } + + points.push_back(s); + points.push_back(h); + indices.push_back(indices.size()); + indices.push_back(indices.size()); + } + + m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],lineColor,points.size(),sizeof(btVector3),&indices[0],indices.size(),1); + } + #if 0 glDisable (GL_LIGHTING); glColor3f (0.0, 1.0, 0.0); @@ -353,6 +391,7 @@ void BenchmarkDemo::initPhysics() m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; //don't recalculate friction values each frame dynamicsWorld->getSolverInfo().m_numIterations = 5; //few solver iterations //m_defaultContactProcessingThreshold = 0.f;//used when creating bodies: body->setContactProcessingThreshold(...); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); @@ -1198,7 +1237,7 @@ void BenchmarkDemo::createTest6() void BenchmarkDemo::initRays() { - raycastBar = btRaycastBar2 (2500.0, 0,50.0); + raycastBar = btRaycastBar2 (2500.0, 0,50.0,m_guiHelper); } void BenchmarkDemo::castRays() diff --git a/examples/Constraints/Dof6Spring2Setup.cpp b/examples/Constraints/Dof6Spring2Setup.cpp index 2531616c7..ba96cc010 100644 --- a/examples/Constraints/Dof6Spring2Setup.cpp +++ b/examples/Constraints/Dof6Spring2Setup.cpp @@ -120,7 +120,8 @@ void Dof6Spring2Setup::initPhysics() m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); m_dynamicsWorld->getDispatchInfo().m_useContinuous = true; - + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(0,0,0)); // Setup a big ground box @@ -188,6 +189,7 @@ void Dof6Spring2Setup::initPhysics() constraint->setDamping(0, 1); #endif constraint->setEquilibriumPoint(0, 0); + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); } @@ -220,6 +222,7 @@ void Dof6Spring2Setup::initPhysics() constraint->setDamping(5, 1); #endif constraint->setEquilibriumPoint(0, 0); + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); } @@ -253,6 +256,7 @@ void Dof6Spring2Setup::initPhysics() #endif constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS); constraint->setLimit(0, -SIMD_INFINITY, 2); @@ -268,6 +272,7 @@ void Dof6Spring2Setup::initPhysics() #endif constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); } @@ -301,6 +306,7 @@ void Dof6Spring2Setup::initPhysics() constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f; constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; #endif + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); } @@ -338,6 +344,7 @@ void Dof6Spring2Setup::initPhysics() constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; //servo motor is not implemented in 6dofspring constraint #endif + constraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(constraint, true); m_data->m_ServoMotorConstraint = constraint; } @@ -378,6 +385,7 @@ void Dof6Spring2Setup::initPhysics() constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.9,a); constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); } + constraint->setDbgDrawSize(btScalar(1.f)); m_dynamicsWorld->addConstraint(constraint, true); if(i < bodycount - 1) @@ -386,7 +394,9 @@ void Dof6Spring2Setup::initPhysics() localB.setIdentity(); CONSTRAINT_TYPE* constraintZY = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS); constraintZY->setLimit(0, 1, -1); + constraintZY->setDbgDrawSize(btScalar(1.f)); m_dynamicsWorld->addConstraint(constraintZY, true); + } } else @@ -404,6 +414,7 @@ void Dof6Spring2Setup::initPhysics() m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a); m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); } + m_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(1.f)); m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); } prevBody = body; @@ -451,12 +462,14 @@ void Dof6Spring2Setup::animate() { m_data->m_ChainRightBody->setActivationState(ACTIVE_TAG); m_dynamicsWorld->removeConstraint(m_data->m_ChainRightConstraint); + m_data->m_ChainLeftConstraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); } else { m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG); m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint); + m_data->m_ChainRightConstraint->setDbgDrawSize(btScalar(2.f)); m_dynamicsWorld->addConstraint(m_data->m_ChainRightConstraint, true); } chainNextFrame = 3.0; diff --git a/examples/DynamicControlDemo/MotorDemo.cpp b/examples/DynamicControlDemo/MotorDemo.cpp new file mode 100644 index 000000000..84284c534 --- /dev/null +++ b/examples/DynamicControlDemo/MotorDemo.cpp @@ -0,0 +1,450 @@ +/* +Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans +Motor Demo + +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 "btBulletDynamicsCommon.h" + +#include "LinearMath/btIDebugDraw.h" +#include "MotorDemo.h" + + +#include "LinearMath/btAlignedObjectArray.h" +class btBroadphaseInterface; +class btCollisionShape; +class btOverlappingPairCache; +class btCollisionDispatcher; +class btConstraintSolver; +struct btCollisionAlgorithmCreateFunc; +class btDefaultCollisionConfiguration; + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + +class MotorDemo : public CommonRigidBodyBase +{ + float m_Time; + float m_fCyclePeriod; // in milliseconds + float m_fMuscleStrength; + + btAlignedObjectArray m_rigs; + + +public: + MotorDemo(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + + void initPhysics(); + + void exitPhysics(); + + virtual ~MotorDemo() + { + } + + void spawnTestRig(const btVector3& startOffset, bool bFixed); + +// virtual void keyboardCallback(unsigned char key, int x, int y); + + void setMotorTargets(btScalar deltaTime); + + void resetCamera() + { + float dist = 11; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } +}; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +#ifndef M_PI_4 +#define M_PI_4 0.785398163397448309616 +#endif + +#ifndef M_PI_8 +#define M_PI_8 0.5 * M_PI_4 +#endif + + +// /LOCAL FUNCTIONS + + + +#define NUM_LEGS 6 +#define BODYPART_COUNT 2 * NUM_LEGS + 1 +#define JOINT_COUNT BODYPART_COUNT - 1 + +class TestRig +{ + btDynamicsWorld* m_ownerWorld; + btCollisionShape* m_shapes[BODYPART_COUNT]; + btRigidBody* m_bodies[BODYPART_COUNT]; + btTypedConstraint* m_joints[JOINT_COUNT]; + + btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) + { + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + m_ownerWorld->addRigidBody(body); + + return body; + } + + +public: + TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed) + : m_ownerWorld (ownerWorld) + { + btVector3 vUp(0, 1, 0); + + // + // Setup geometry + // + float fBodySize = 0.25f; + float fLegLength = 0.45f; + float fForeLegLength = 0.75f; + m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10)); + int i; + for ( i=0; isetDamping(0.05, 0.85); + m_bodies[i]->setDeactivationTime(0.8); + //m_bodies[i]->setSleepingThresholds(1.6, 2.5); + m_bodies[i]->setSleepingThresholds(0.5f, 0.5f); + } + + + // + // Setup the constraints + // + btHingeConstraint* hingeC; + //btConeTwistConstraint* coneC; + + btTransform localA, localB, localC; + + for ( i=0; igetWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; + hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB); + hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8)); + //hingeC->setLimit(btScalar(-0.1), btScalar(0.1)); + m_joints[2*i] = hingeC; + m_ownerWorld->addConstraint(m_joints[2*i], true); + + // knee joints + localA.setIdentity(); localB.setIdentity(); localC.setIdentity(); + localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(0.), btScalar(fSin*(fBodySize+fLegLength)))); + localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; + localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; + hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC); + //hingeC->setLimit(btScalar(-0.01), btScalar(0.01)); + hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2)); + m_joints[1+2*i] = hingeC; + m_ownerWorld->addConstraint(m_joints[1+2*i], true); + } + } + + virtual ~TestRig () + { + int i; + + // Remove all constraints + for ( i = 0; i < JOINT_COUNT; ++i) + { + m_ownerWorld->removeConstraint(m_joints[i]); + delete m_joints[i]; m_joints[i] = 0; + } + + // Remove all bodies and shapes + for ( i = 0; i < BODYPART_COUNT; ++i) + { + m_ownerWorld->removeRigidBody(m_bodies[i]); + + delete m_bodies[i]->getMotionState(); + + delete m_bodies[i]; m_bodies[i] = 0; + delete m_shapes[i]; m_shapes[i] = 0; + } + } + + btTypedConstraint** GetJoints() {return &m_joints[0];} + +}; + + + +void motorPreTickCallback (btDynamicsWorld *world, btScalar timeStep) +{ + MotorDemo* motorDemo = (MotorDemo*)world->getWorldUserInfo(); + + motorDemo->setMotorTargets(timeStep); + +} + + + +void MotorDemo::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + // Setup the basic world + + m_Time = 0; + m_fCyclePeriod = 2000.f; // in milliseconds + +// m_fMuscleStrength = 0.05f; + // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor + // should be (numberOfsolverIterations * oldLimits) + // currently solver uses 10 iterations, so: + m_fMuscleStrength = 0.5f; + + + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + btVector3 worldAabbMin(-10000,-10000,-10000); + btVector3 worldAabbMax(10000,10000,10000); + m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax); + + m_solver = new btSequentialImpulseConstraintSolver; + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + + m_dynamicsWorld->setInternalTickCallback(motorPreTickCallback,this,true); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + + // Setup a big ground box + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.))); + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-10,0)); + createRigidBody(btScalar(0.),groundTransform,groundShape); + } + + // Spawn one ragdoll + btVector3 startOffset(1,0.5,0); + spawnTestRig(startOffset, false); + startOffset.setValue(-2,0.5,0); + spawnTestRig(startOffset, true); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void MotorDemo::spawnTestRig(const btVector3& startOffset, bool bFixed) +{ + TestRig* rig = new TestRig(m_dynamicsWorld, startOffset, bFixed); + m_rigs.push_back(rig); +} + +void PreStep() +{ + +} + + + + +void MotorDemo::setMotorTargets(btScalar deltaTime) +{ + + float ms = deltaTime*1000000.; + float minFPS = 1000000.f/60.f; + if (ms > minFPS) + ms = minFPS; + + m_Time += ms; + + // + // set per-frame sinusoidal position targets using angular motor (hacky?) + // + for (int r=0; r(m_rigs[r]->GetJoints()[i]); + btScalar fCurAngle = hingeC->getHingeAngle(); + + btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod; + btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent)); + btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); + btScalar fAngleError = fTargetLimitAngle - fCurAngle; + btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms; + hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength); + } + } + + +} + +#if 0 +void MotorDemo::keyboardCallback(unsigned char key, int x, int y) +{ + switch (key) + { + case '+': case '=': + m_fCyclePeriod /= 1.1f; + if (m_fCyclePeriod < 1.f) + m_fCyclePeriod = 1.f; + break; + case '-': case '_': + m_fCyclePeriod *= 1.1f; + break; + case '[': + m_fMuscleStrength /= 1.1f; + break; + case ']': + m_fMuscleStrength *= 1.1f; + break; + default: + DemoApplication::keyboardCallback(key, x, y); + } +} +#endif + + + +void MotorDemo::exitPhysics() +{ + + int i; + + for (i=0;igetNumCollisionObjects()-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;j //printf debugging + + +int sFrameNumber = 0; + +#include "btFractureBody.h" +#include "btFractureDynamicsWorld.h" + + +#include "LinearMath/btAlignedObjectArray.h" + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + + +///FractureDemo shows basic breaking and glueing of objects +class FractureDemo : public CommonRigidBodyBase +{ + + +public: + + FractureDemo(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + virtual ~FractureDemo() + { + } + void initPhysics(); + + void exitPhysics(); + + virtual void stepSimulation(float deltaTime) + { + CommonRigidBodyBase::stepSimulation(deltaTime); + + { + BT_PROFILE("recreate graphics"); + //@todo: make this graphics re-creation better + //right now: brute force remove all graphics objects, and re-create them every frame + m_guiHelper->getRenderInterface()->removeAllInstances(); + for (int i=0;igetNumCollisionObjects();i++) + { + btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i]; + colObj->getCollisionShape()->setUserIndex(-1); + colObj->setUserIndex(-1); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + } + } + + virtual bool keyboardCallback(int key, int state); + + void resetCamera() + { + float dist = 41; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + +}; + + + + +void FractureDemo::initPhysics() +{ + + m_guiHelper->setUpAxis(1); + + + ///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(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + m_solver = sol; + + //m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + + btFractureDynamicsWorld* fractureWorld = new btFractureDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld = fractureWorld; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + + //m_splitImpulse removes the penetration resolution from the applied impulse, otherwise objects might fracture due to deep penetrations. + m_dynamicsWorld->getSolverInfo().m_splitImpulse = true; + + { + ///create a few basic rigid bodies + btCollisionShape* groundShape = new btBoxShape(btVector3(50,1,50)); + /// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),0); + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,0,0)); + createRigidBody(0.f,groundTransform,groundShape); + } + + { + ///create a few basic rigid bodies + btCollisionShape* shape = new btBoxShape(btVector3(1,1,1)); + m_collisionShapes.push_back(shape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(5,2,0)); + createRigidBody(0.f,tr,shape); + } + + + + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); + //btCollisionShape* colShape = new btCapsuleShape(SCALING*0.4,SCALING*1); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //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) + colShape->calculateLocalInertia(mass,localInertia); + + + int gNumObjects = 10; + + for (int i=0;isetLinearVelocity(btVector3(0,-10,0)); + + m_dynamicsWorld->addRigidBody(body); + + } + + } + + + + fractureWorld->stepSimulation(1./60.,0); + fractureWorld->glueCallback(); + + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +#if 0 +void FractureDemo::showMessage() +{ + if((getDebugMode() & btIDebugDraw::DBG_DrawText)) + { + setOrthographicProjection(); + glDisable(GL_LIGHTING); + glColor3f(0, 0, 0); + char buf[124]; + + int lineWidth=380; + int xStart = m_glutScreenWidth - lineWidth; + int yStart = 20; + + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + if (world->getFractureMode()) + { + sprintf(buf,"Fracture mode"); + } else + { + sprintf(buf,"Glue mode"); + } + GLDebugDrawString(xStart,yStart,buf); + sprintf(buf,"f to toggle fracture/glue mode"); + yStart+=20; + GLDebugDrawString(xStart,yStart,buf); + sprintf(buf,"space to restart, mouse to pick/shoot"); + yStart+=20; + GLDebugDrawString(xStart,yStart,buf); + + resetPerspectiveProjection(); + glEnable(GL_LIGHTING); + } + +} +#endif + +#if 0 +void FractureDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + + showMessage(); + + //optional but useful: debug drawing to detect problems + if (m_dynamicsWorld) + m_dynamicsWorld->debugDrawWorld(); + + glFlush(); + swapBuffers(); +} +#endif + + +bool FractureDemo::keyboardCallback(int key, int state) +{ + + if (key=='f' && (state==0)) + { + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + world->setFractureMode(!world->getFractureMode()); + if (world->getFractureMode()) + { + b3Printf("Fracturing mode"); + } else + { + b3Printf("Gluing mode"); + + } + return true; + } + + return false; +} + + +#if 0 +void FractureDemo::keyboardUpCallback(unsigned char key, int x, int y) +{ + if (key=='f') + { + btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld; + world->setFractureMode(!world->getFractureMode()); + } + + PlatformDemoApplication::keyboardUpCallback(key,x,y); + +} +#endif + +#if 0 +void FractureDemo::shootBox(const btVector3& destination) +{ + + if (m_dynamicsWorld) + { + btScalar mass = 1.f; + btTransform startTransform; + startTransform.setIdentity(); + btVector3 camPos = getCameraPosition(); + startTransform.setOrigin(camPos); + + setShootBoxShape (); + + btAssert((!m_shootBoxShape || m_shootBoxShape->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) + m_shootBoxShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + + btFractureBody* body = new btFractureBody(mass,0,m_shootBoxShape,localInertia,&mass,1,m_dynamicsWorld); + + body->setWorldTransform(startTransform); + + m_dynamicsWorld->addRigidBody(body); + + + body->setLinearFactor(btVector3(1,1,1)); + //body->setRestitution(1); + + btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]); + linVel.normalize(); + linVel*=m_ShootBoxInitialSpeed; + + body->getWorldTransform().setOrigin(camPos); + body->getWorldTransform().setRotation(btQuaternion(0,0,0,1)); + body->setLinearVelocity(linVel); + body->setAngularVelocity(btVector3(0,0,0)); + body->setCcdMotionThreshold(1.); + body->setCcdSweptSphereRadius(0.2f); + + } +} +#endif + + + + + + + +void FractureDemo::exitPhysics() +{ + + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int 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;jisCompound()) + { + btCompoundShape* compound = (btCompoundShape*)getCollisionShape(); + for (int i=0;igetNumChildShapes();i++) + { + for (int j=i+1;jgetNumChildShapes();j++) + { + + struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback + { + bool m_connected; + btScalar m_margin; + MyContactResultCallback() :m_connected(false),m_margin(0.05) + { + } + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) + { + if (cp.getDistance()<=m_margin) + m_connected = true; + return 1.f; + } + }; + + MyContactResultCallback result; + + btCollisionObject obA; + obA.setWorldTransform(compound->getChildTransform(i)); + obA.setCollisionShape(compound->getChildShape(i)); + btCollisionObject obB; + obB.setWorldTransform(compound->getChildTransform(j)); + obB.setCollisionShape(compound->getChildShape(j)); + world->contactPairTest(&obA,&obB,result); + if (result.m_connected) + { + btConnection tmp; + tmp.m_childIndex0 = i; + tmp.m_childIndex1 = j; + tmp.m_childShape0 = compound->getChildShape(i); + tmp.m_childShape1 = compound->getChildShape(j); + tmp.m_strength = 1.f;//?? + m_connections.push_back(tmp); + } + } + } + } + + +} + +btCompoundShape* btFractureBody::shiftTransformDistributeMass(btCompoundShape* boxCompound,btScalar mass,btTransform& shift) +{ + + btVector3 principalInertia; + + btScalar* masses = new btScalar[boxCompound->getNumChildShapes()]; + for (int j=0;jgetNumChildShapes();j++) + { + //evenly distribute mass + masses[j]=mass/boxCompound->getNumChildShapes(); + } + + return shiftTransform(boxCompound,masses,shift,principalInertia); + +} + + +btCompoundShape* btFractureBody::shiftTransform(btCompoundShape* boxCompound,btScalar* masses,btTransform& shift, btVector3& principalInertia) +{ + btTransform principal; + + boxCompound->calculatePrincipalAxisTransform(masses,principal,principalInertia); + + + ///create a new compound with world transform/center of mass properly aligned with the principal axis + + ///non-recursive compound shapes perform better + +#ifdef USE_RECURSIVE_COMPOUND + + btCompoundShape* newCompound = new btCompoundShape(); + newCompound->addChildShape(principal.inverse(),boxCompound); + newBoxCompound = newCompound; + //m_collisionShapes.push_back(newCompound); + + //btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + //btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,newCompound,principalInertia); + +#else +#ifdef CHANGE_COMPOUND_INPLACE + newBoxCompound = boxCompound; + for (int i=0;igetNumChildShapes();i++) + { + btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i); + ///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update + boxCompound->updateChildTransform(i,newChildTransform); + } + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0,0,0); + if (isDynamic) + boxCompound->calculateLocalInertia(mass,localInertia); + +#else + ///creation is faster using a new compound to store the shifted children + btCompoundShape* newBoxCompound = new btCompoundShape(); + for (int i=0;igetNumChildShapes();i++) + { + btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i); + ///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update + newBoxCompound->addChildShape(newChildTransform,boxCompound->getChildShape(i)); + } + + + +#endif + +#endif//USE_RECURSIVE_COMPOUND + + shift = principal; + return newBoxCompound; +} + + + + + + diff --git a/examples/FractureDemo/btFractureBody.h b/examples/FractureDemo/btFractureBody.h new file mode 100644 index 000000000..922db3c22 --- /dev/null +++ b/examples/FractureDemo/btFractureBody.h @@ -0,0 +1,78 @@ + +#ifndef BT_FRACTURE_BODY +#define BT_FRACTURE_BODY + +class btCollisionShape; +class btDynamicsWorld; +class btCollisionWorld; +class btCompoundShape; +class btManifoldPoint; + +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +#define CUSTOM_FRACTURE_TYPE (btRigidBody::CO_USER_TYPE+1) + + +struct btConnection +{ + + btCollisionShape* m_childShape0; + btCollisionShape* m_childShape1; + int m_childIndex0; + int m_childIndex1; + btScalar m_strength; +}; + +class btFractureBody : public btRigidBody +{ + //connections +public: + + btDynamicsWorld* m_world; + btAlignedObjectArray m_masses; + btAlignedObjectArray m_connections; + + + + btFractureBody( const btRigidBodyConstructionInfo& constructionInfo, btDynamicsWorld* world) + :btRigidBody(constructionInfo), + m_world(world) + { + m_masses.push_back(constructionInfo.m_mass); + m_internalType=CUSTOM_FRACTURE_TYPE+CO_RIGID_BODY; + } + + + + ///btRigidBody constructor for backwards compatibility. + ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) + btFractureBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia, btScalar* masses, int numMasses, btDynamicsWorld* world) + :btRigidBody(mass,motionState,collisionShape,localInertia), + m_world(world) + { + + for (int i=0;igetNumManifolds(); + + ///first build the islands based on axis aligned bounding box overlap + + btUnionFind unionFind; + + int index = 0; + { + + int i; + for (i=0;iisStaticOrKinematicObject()) + { + collisionObject->setIslandTag(index++); + } else + { + collisionObject->setIslandTag(-1); + } +#else + collisionObject->setIslandTag(i); + index=i+1; +#endif + } + } + + unionFind.reset(index); + + int numElem = unionFind.getNumElements(); + + for (int i=0;igetManifoldByIndexInternal(i); + if (!manifold->getNumContacts()) + continue; + + btScalar minDist = 1e30f; + for (int v=0;vgetNumContacts();v++) + { + minDist = btMin(minDist,manifold->getContactPoint(v).getDistance()); + } + if (minDist>0.) + continue; + + btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0(); + btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1(); + int tag0 = (colObj0)->getIslandTag(); + int tag1 = (colObj1)->getIslandTag(); + //btRigidBody* body0 = btRigidBody::upcast(colObj0); + //btRigidBody* body1 = btRigidBody::upcast(colObj1); + + + if (!colObj0->isStaticOrKinematicObject() && !colObj1->isStaticOrKinematicObject()) + { + unionFind.unite(tag0, tag1); + } + } + + + + + numElem = unionFind.getNumElements(); + + + + index=0; + for (int ai=0;aiisStaticOrKinematicObject()) + { + int tag = unionFind.find(index); + + collisionObject->setIslandTag( tag); + + //Set the correct object offset in Collision Object Array +#if STATIC_SIMULATION_ISLAND_OPTIMIZATION + unionFind.getElement(index).m_sz = ai; +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION + + index++; + } + } + unionFind.sortIslands(); + + + + int endIslandIndex=1; + int startIslandIndex; + + btAlignedObjectArray removedObjects; + + ///iterate over all islands + for ( startIslandIndex=0;startIslandIndexgetInternalType()& CUSTOM_FRACTURE_TYPE) + { + fractureObjectIndex = i; + } + btRigidBody* otherObject = btRigidBody::upcast(colObj0); + if (!otherObject || !otherObject->getInvMass()) + continue; + numObjects++; + } + + ///Then for each island that contains at least two objects and one fracture object + if (fractureObjectIndex>=0 && numObjects>1) + { + + btFractureBody* fracObj = (btFractureBody*)getCollisionObjectArray()[fractureObjectIndex]; + + ///glueing objects means creating a new compound and removing the old objects + ///delay the removal of old objects to avoid array indexing problems + removedObjects.push_back(fracObj); + m_fractureBodies.remove(fracObj); + + btAlignedObjectArray massArray; + + btAlignedObjectArray oldImpulses; + btAlignedObjectArray oldCenterOfMassesWS; + + oldImpulses.push_back(fracObj->getLinearVelocity()/1./fracObj->getInvMass()); + oldCenterOfMassesWS.push_back(fracObj->getCenterOfMassPosition()); + + btScalar totalMass = 0.f; + + + btCompoundShape* compound = new btCompoundShape(); + if (fracObj->getCollisionShape()->isCompound()) + { + btTransform tr; + tr.setIdentity(); + btCompoundShape* oldCompound = (btCompoundShape*)fracObj->getCollisionShape(); + for (int c=0;cgetNumChildShapes();c++) + { + compound->addChildShape(oldCompound->getChildTransform(c),oldCompound->getChildShape(c)); + massArray.push_back(fracObj->m_masses[c]); + totalMass+=fracObj->m_masses[c]; + } + + } else + { + btTransform tr; + tr.setIdentity(); + compound->addChildShape(tr,fracObj->getCollisionShape()); + massArray.push_back(fracObj->m_masses[0]); + totalMass+=fracObj->m_masses[0]; + } + + for (idx=startIslandIndex;idxgetInvMass()) + continue; + + + oldImpulses.push_back(otherObject->getLinearVelocity()*(1.f/otherObject->getInvMass())); + oldCenterOfMassesWS.push_back(otherObject->getCenterOfMassPosition()); + + removedObjects.push_back(otherObject); + m_fractureBodies.remove((btFractureBody*)otherObject); + + btScalar curMass = 1.f/otherObject->getInvMass(); + + + if (otherObject->getCollisionShape()->isCompound()) + { + btTransform tr; + btCompoundShape* oldCompound = (btCompoundShape*)otherObject->getCollisionShape(); + for (int c=0;cgetNumChildShapes();c++) + { + tr = fracObj->getWorldTransform().inverseTimes(otherObject->getWorldTransform()*oldCompound->getChildTransform(c)); + compound->addChildShape(tr,oldCompound->getChildShape(c)); + massArray.push_back(curMass/(btScalar)oldCompound->getNumChildShapes()); + + } + } else + { + btTransform tr; + tr = fracObj->getWorldTransform().inverseTimes(otherObject->getWorldTransform()); + compound->addChildShape(tr,otherObject->getCollisionShape()); + massArray.push_back(curMass); + } + totalMass+=curMass; + } + + + + btTransform shift; + shift.setIdentity(); + btCompoundShape* newCompound = btFractureBody::shiftTransformDistributeMass(compound,totalMass,shift); + int numChildren = newCompound->getNumChildShapes(); + btAssert(numChildren == massArray.size()); + + btVector3 localInertia; + newCompound->calculateLocalInertia(totalMass,localInertia); + btFractureBody* newBody = new btFractureBody(totalMass,0,newCompound,localInertia, &massArray[0], numChildren,this); + newBody->recomputeConnectivity(this); + newBody->setWorldTransform(fracObj->getWorldTransform()*shift); + + //now the linear/angular velocity is still zero, apply the impulses + + for (int i=0;igetCenterOfMassPosition(); + const btVector3& imp = oldImpulses[i]; + newBody->applyImpulse(imp, rel_pos); + } + + addRigidBody(newBody); + + + } + + + } + + //remove the objects from the world at the very end, + //otherwise the island tags would not match the world collision object array indices anymore + while (removedObjects.size()) + { + btCollisionObject* otherCollider = removedObjects[removedObjects.size()-1]; + removedObjects.pop_back(); + + btRigidBody* otherObject = btRigidBody::upcast(otherCollider); + if (!otherObject || !otherObject->getInvMass()) + continue; + removeRigidBody(otherObject); + } + +} + + +struct btFracturePair +{ + btFractureBody* m_fracObj; + btAlignedObjectArray m_contactManifolds; +}; + + + +void btFractureDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + // todo: after fracture we should run the solver again for better realism + // for example + // save all velocities and if one or more objects fracture: + // 1) revert all velocties + // 2) apply impulses for the fracture bodies at the contact locations + // 3)and run the constaint solver again + + btDiscreteDynamicsWorld::solveConstraints(solverInfo); + + fractureCallback(); +} + +btFractureBody* btFractureDynamicsWorld::addNewBody(const btTransform& oldTransform,btScalar* masses, btCompoundShape* oldCompound) +{ + int i; + + btTransform shift; + shift.setIdentity(); + btVector3 localInertia; + btCompoundShape* newCompound = btFractureBody::shiftTransform(oldCompound,masses,shift,localInertia); + btScalar totalMass = 0; + for (i=0;igetNumChildShapes();i++) + totalMass += masses[i]; + //newCompound->calculateLocalInertia(totalMass,localInertia); + + btFractureBody* newBody = new btFractureBody(totalMass,0,newCompound,localInertia, masses,newCompound->getNumChildShapes(), this); + newBody->recomputeConnectivity(this); + + newBody->setCollisionFlags(newBody->getCollisionFlags()|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + newBody->setWorldTransform(oldTransform*shift); + addRigidBody(newBody); + return newBody; +} + +void btFractureDynamicsWorld::addRigidBody(btRigidBody* body) +{ + if (body->getInternalType() & CUSTOM_FRACTURE_TYPE) + { + btFractureBody* fbody = (btFractureBody*)body; + m_fractureBodies.push_back(fbody); + } + btDiscreteDynamicsWorld::addRigidBody(body); +} + +void btFractureDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + if (body->getInternalType() & CUSTOM_FRACTURE_TYPE) + { + btFractureBody* fbody = (btFractureBody*)body; + btAlignedObjectArray tmpConstraints; + + for (int i=0;igetNumConstraintRefs();i++) + { + tmpConstraints.push_back(fbody->getConstraintRef(i)); + } + + //remove all constraints attached to this rigid body too + for (int i=0;igetCollisionShape()->isCompound()) + return; + + btCompoundShape* compound = (btCompoundShape*)fracObj->getCollisionShape(); + int numChildren = compound->getNumChildShapes(); + + if (numChildren<=1) + return; + + //compute connectivity + btUnionFind unionFind; + + btAlignedObjectArray tags; + tags.resize(numChildren); + int i, index = 0; + for ( i=0;im_connections.size();i++) + { + btConnection& connection = fracObj->m_connections[i]; + if (connection.m_strength > 0.) + { + int tag0 = tags[connection.m_childIndex0]; + int tag1 = tags[connection.m_childIndex1]; + unionFind.unite(tag0, tag1); + } + } + numElem = unionFind.getNumElements(); + + index=0; + for (int ai=0;ai removedObjects; + + int numIslands = 0; + + for ( startIslandIndex=0;startIslandIndex masses; + + int idx; + for (idx=startIslandIndex;idxgetChildShape(i); + newCompound->addChildShape(compound->getChildTransform(i),compound->getChildShape(i)); + masses.push_back(fracObj->m_masses[i]); + numShapes++; + } + if (numShapes) + { + btFractureBody* newBody = addNewBody(fracObj->getWorldTransform(),&masses[0],newCompound); + newBody->setLinearVelocity(fracObj->getLinearVelocity()); + newBody->setAngularVelocity(fracObj->getAngularVelocity()); + + numIslands++; + } + } + + + + + + removeRigidBody(fracObj);//should it also be removed from the array? + + +} + +#include + + +void btFractureDynamicsWorld::fractureCallback( ) +{ + + btAlignedObjectArray sFracturePairs; + + if (!m_fracturingMode) + { + glueCallback(); + return; + } + + int numManifolds = getDispatcher()->getNumManifolds(); + + sFracturePairs.clear(); + + + for (int i=0;igetManifoldByIndexInternal(i); + if (!manifold->getNumContacts()) + continue; + + btScalar totalImpact = 0.f; + for (int p=0;pgetNumContacts();p++) + { + totalImpact += manifold->getContactPoint(p).m_appliedImpulse; + } + + +// printf("totalImpact=%f\n",totalImpact); + + static float maxImpact = 0; + if (totalImpact>maxImpact) + maxImpact = totalImpact; + + //some threshold otherwise resting contact would break objects after a while + if (totalImpact < 40.f) + continue; + + // printf("strong impact\n"); + + + //@todo: add better logic to decide what parts to fracture + //For example use the idea from the SIGGRAPH talk about the fracture in the movie 2012: + // + //Breaking thresholds can be stored as connectivity information between child shapes in the fracture object + // + //You can calculate some "impact value" by simulating all the individual child shapes + //as rigid bodies, without constraints, running it in a separate simulation world + //(or by running the constraint solver without actually modifying the dynamics world) + //Then measure some "impact value" using the offset and applied impulse for each child shape + //weaken the connections based on this "impact value" and only break + //if this impact value exceeds the breaking threshold. + //you can propagate the weakening and breaking of connections using the connectivity information + + int f0 = m_fractureBodies.findLinearSearch((btFractureBody*)manifold->getBody0()); + int f1 = m_fractureBodies.findLinearSearch((btFractureBody*)manifold->getBody1()); + + if (f0 == f1 == m_fractureBodies.size()) + continue; + + + if (f0getBody1(); + // btRigidBody* otherOb = btRigidBody::upcast(colOb); + // if (!otherOb->getInvMass()) + // continue; + + int pi=-1; + + for (int p=0;pgetBody0(); + btRigidBody* otherOb = btRigidBody::upcast(colOb); + // if (!otherOb->getInvMass()) + // continue; + + + int pi=-1; + + for (int p=0;pgetCollisionShape()->isCompound()) + { + btTransform tr; + tr.setIdentity(); + btCompoundShape* oldCompound = (btCompoundShape*)sFracturePairs[i].m_fracObj->getCollisionShape(); + if (oldCompound->getNumChildShapes()>1) + { + bool needsBreakingCheck = false; + + + //weaken/break the connections + + //@todo: propagate along the connection graph + for (int j=0;jgetNumContacts();k++) + { + btManifoldPoint& pt = manifold->getContactPoint(k); + if (manifold->getBody0()==sFracturePairs[i].m_fracObj) + { + for (int f=0;fm_connections.size();f++) + { + btConnection& connection = sFracturePairs[i].m_fracObj->m_connections[f]; + if ( (connection.m_childIndex0 == pt.m_index0) || + (connection.m_childIndex1 == pt.m_index0) + ) + { + connection.m_strength -= pt.m_appliedImpulse; + if (connection.m_strength<0) + { + //remove or set to zero + connection.m_strength=0.f; + needsBreakingCheck = true; + } + } + } + } else + { + for (int f=0;fm_connections.size();f++) + { + btConnection& connection = sFracturePairs[i].m_fracObj->m_connections[f]; + if ( (connection.m_childIndex0 == pt.m_index1) || + (connection.m_childIndex1 == pt.m_index1) + ) + { + connection.m_strength -= pt.m_appliedImpulse; + if (connection.m_strength<0) + { + //remove or set to zero + connection.m_strength=0.f; + needsBreakingCheck = true; + } + } + } + } + } + } + + if (needsBreakingCheck) + { + breakDisconnectedParts(sFracturePairs[i].m_fracObj); + } + } + + } + + } + } + + sFracturePairs.clear(); + +} + diff --git a/examples/FractureDemo/btFractureDynamicsWorld.h b/examples/FractureDemo/btFractureDynamicsWorld.h new file mode 100644 index 000000000..255487729 --- /dev/null +++ b/examples/FractureDemo/btFractureDynamicsWorld.h @@ -0,0 +1,51 @@ +#ifndef _BT_FRACTURE_DYNAMICS_WORLD_H +#define _BT_FRACTURE_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btFractureBody; +class btCompoundShape; +class btTransform; + + +///The btFractureDynamicsWorld class enabled basic glue and fracture of objects. +///If/once this implementation is stablized/tested we might merge it into btDiscreteDynamicsWorld and remove the class. +class btFractureDynamicsWorld : public btDiscreteDynamicsWorld +{ + btAlignedObjectArray m_fractureBodies; + + bool m_fracturingMode; + + btFractureBody* addNewBody(const btTransform& oldTransform,btScalar* masses, btCompoundShape* oldCompound); + + void breakDisconnectedParts( btFractureBody* fracObj); + +public: + + btFractureDynamicsWorld ( btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual void addRigidBody(btRigidBody* body); + + virtual void removeRigidBody(btRigidBody* body); + + void solveConstraints(btContactSolverInfo& solverInfo); + + ///either fracture or glue (!fracture) + void setFractureMode(bool fracture) + { + m_fracturingMode = fracture; + } + + bool getFractureMode() const { return m_fracturingMode;} + + ///normally those callbacks are called internally by the 'solveConstraints' + void glueCallback(); + + ///normally those callbacks are called internally by the 'solveConstraints' + void fractureCallback(); + +}; + +#endif //_BT_FRACTURE_DYNAMICS_WORLD_H + diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index 544cf2a37..66c672ab1 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -1054,7 +1054,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) NSOpenGLContext *foo = [NSOpenGLContext currentContext]; // get the url of a .txt file NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel]; - NSArray * zAryOfExtensions = [NSArray arrayWithObject:@"urdf"]; + NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",nil]; [zOpenPanel setAllowedFileTypes:zAryOfExtensions]; NSInteger zIntResult = [zOpenPanel runModal]; diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp new file mode 100644 index 000000000..afc3956ec --- /dev/null +++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp @@ -0,0 +1,286 @@ +/* +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. +*/ + + +///create 125 (5x5x5) dynamic object +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024) + +///scaling of the objects (0.1 = 20 centimeter boxes ) +#define SCALING 1. +#define START_POS_X -5 +#define START_POS_Y -5 +#define START_POS_Z -3 + +#include "RollingFrictionDemo.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include //printf debugging + + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + + +///The RollingFrictionDemo shows the use of rolling friction. +///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. +///Generally it is best to leave the rolling friction coefficient zero (or close to zero). +class RollingFrictionDemo : public CommonRigidBodyBase +{ +public: + + RollingFrictionDemo(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + virtual ~RollingFrictionDemo() + { + + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 35; + float pitch = 0; + float yaw = 14; + float targetPos[3]={0,0,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + +}; + + + + + + +void RollingFrictionDemo::initPhysics() +{ + + m_guiHelper->setUpAxis(1); + + + ///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(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + m_solver = sol; + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); +// m_dynamicsWorld->getSolverInfo().m_singleAxisRollingFrictionThreshold = 0.f;//faster but lower quality + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + + ///create a few basic rigid bodies + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(20.),btScalar(50.),btScalar(10.))); + + // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + groundTransform.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.03)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //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) + groundShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + body->setRollingFriction(1); + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + { + + ///create a few basic rigid bodies + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.),btScalar(50.),btScalar(100.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-54,0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //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) + groundShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + body->setRollingFriction(1); + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance +#define NUM_SHAPES 10 + btCollisionShape* colShapes[NUM_SHAPES] = { + new btSphereShape(btScalar(1.)), + new btCapsuleShape(0.5,1), + new btCapsuleShapeX(0.5,1), + new btCapsuleShapeZ(0.5,1), + new btConeShape(0.5,1), + new btConeShapeX(0.5,1), + new btConeShapeZ(0.5,1), + new btCylinderShape(btVector3(0.5,1,0.5)), + new btCylinderShapeX(btVector3(1,0.5,0.5)), + new btCylinderShapeZ(btVector3(0.5,0.5,1)), + }; + for (int i=0;icalculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1.f); + body->setRollingFriction(.3); + body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(),btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + + m_dynamicsWorld->addRigidBody(body); + } + } + } + } + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + +} + + + +void RollingFrictionDemo::exitPhysics() +{ + + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int 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;jm_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 e1c9430fd..bff2008b2 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -16,33 +16,18 @@ subject to the following restrictions: #ifndef BT_FIXED_CONSTRAINT_H #define BT_FIXED_CONSTRAINT_H -#include "btTypedConstraint.h" +#include "btGeneric6DofSpring2Constraint.h" -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint + +ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint { - 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