From 3b83428a7ff5851fc2cad10d841384e2cd18346e Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Sun, 9 Sep 2012 17:22:30 +0000 Subject: [PATCH] Applied polar decomposition patch. Fixes Issue 621. Thanks to Christian for the report, Joshua for the fix, Dongsoo for checking the fix. Applied picking cloth patch. Fixes Issue 646. Thanks to Dongsoo. Applied patch Softbody updateConstraints. Fixes Issue 503. Thanks to Dave Bruce Phillips and Dongsoo. Fix various warnigns under Mac OSX. --- .../ConcaveRaycastDemo/ConcaveRaycastDemo.cpp | 2 +- Demos/ForkLiftDemo/ForkLiftDemo.cpp | 8 +- .../FractureDemo/btFractureDynamicsWorld.cpp | 2 +- Demos/OpenCLClothDemo/cl_cloth_demo.cpp | 7 +- Demos/OpenCLClothDemo/gl_win.cpp | 2 +- Demos/OpenGL/GLDebugDrawer.cpp | 15 +- Demos/OpenGL/GLDebugDrawer.h | 1 - Demos/SharedOpenCL/btOpenCLUtils.cpp | 18 +- .../btBulletWorldImporter.cpp | 2 +- UnitTests/BulletUnitTests/CMakeLists.txt | 23 ++- UnitTests/BulletUnitTests/Main.cpp | 10 +- .../TestCholeskyDecomposition.cpp | 68 +++++++ .../TestCholeskyDecomposition.h | 52 +++++ UnitTests/BulletUnitTests/TestLinearMath.h | 6 +- .../TestPolarDecomposition.cpp | 103 ++++++++++ .../BulletUnitTests/TestPolarDecomposition.h | 88 ++++++++ .../btCholeskyDecomposition.cpp | 40 ++++ .../BulletUnitTests/btCholeskyDecomposition.h | 19 ++ .../CollisionDispatch/btCollisionWorld.cpp | 8 +- .../btEmptyCollisionAlgorithm.h | 8 +- src/BulletCollision/Gimpact/btGImpactShape.h | 2 +- .../btSequentialImpulseConstraintSolver.cpp | 19 +- .../ConstraintSolver/btTypedConstraint.h | 2 +- .../OpenCL/btSoftBodySolver_OpenCL.cpp | 6 +- .../SpuContactManifoldCollisionAlgorithm.cpp | 2 +- .../SpuContactManifoldCollisionAlgorithm.h | 17 +- .../btGpu3DGridBroadphase.cpp | 2 +- .../btGpu3DGridBroadphase.h | 4 +- .../btParallelConstraintSolver.cpp | 3 +- src/BulletSoftBody/btSoftBody.cpp | 190 +++++++++++++----- src/BulletSoftBody/btSoftBody.h | 11 + src/BulletSoftBody/btSoftBodyInternals.h | 33 +-- .../btSoftRigidCollisionAlgorithm.cpp | 4 +- src/LinearMath/CMakeLists.txt | 2 + src/LinearMath/btIDebugDraw.h | 1 + src/LinearMath/btPolarDecomposition.cpp | 99 +++++++++ src/LinearMath/btPolarDecomposition.h | 73 +++++++ src/Makefile.am | 3 + src/MiniCL/MiniCL.cpp | 8 +- src/MiniCL/cl_MiniCL_Defs.h | 2 +- 40 files changed, 811 insertions(+), 154 deletions(-) create mode 100644 UnitTests/BulletUnitTests/TestCholeskyDecomposition.cpp create mode 100644 UnitTests/BulletUnitTests/TestCholeskyDecomposition.h create mode 100644 UnitTests/BulletUnitTests/TestPolarDecomposition.cpp create mode 100644 UnitTests/BulletUnitTests/TestPolarDecomposition.h create mode 100644 UnitTests/BulletUnitTests/btCholeskyDecomposition.cpp create mode 100644 UnitTests/BulletUnitTests/btCholeskyDecomposition.h create mode 100644 src/LinearMath/btPolarDecomposition.cpp create mode 100644 src/LinearMath/btPolarDecomposition.h diff --git a/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp b/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp index 9f0e1d6c0..25242b7ae 100644 --- a/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp +++ b/Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp @@ -83,7 +83,7 @@ public: this->min_y = min_y; this->max_y = max_y; sign = 1.0; - btScalar dalpha = 2*SIMD_2_PI/NUMRAYS_IN_BAR; + // btScalar dalpha = 2*SIMD_2_PI/NUMRAYS_IN_BAR; for (int i = 0; i < NUMRAYS_IN_BAR; i++) { btScalar z = (max_z-min_z)/btScalar(NUMRAYS_IN_BAR) * btScalar(i) + min_z; diff --git a/Demos/ForkLiftDemo/ForkLiftDemo.cpp b/Demos/ForkLiftDemo/ForkLiftDemo.cpp index cc761d9d5..3d5c4d925 100644 --- a/Demos/ForkLiftDemo/ForkLiftDemo.cpp +++ b/Demos/ForkLiftDemo/ForkLiftDemo.cpp @@ -107,11 +107,11 @@ m_carChassis(0), m_liftBody(0), m_forkBody(0), m_loadBody(0), +m_indexVertexArrays(0), +m_vertices(0), m_cameraHeight(4.f), m_minCameraDistance(3.f), -m_maxCameraDistance(10.f), -m_indexVertexArrays(0), -m_vertices(0) +m_maxCameraDistance(10.f) { m_vehicle = 0; m_wheelShape = 0; @@ -507,11 +507,11 @@ void ForkLiftDemo::clientMoveAndDisplay() if (m_idle) dt = 1.0/420.f; - int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); //#define VERBOSE_FEEDBACK #ifdef VERBOSE_FEEDBACK + int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); if (!numSimSteps) printf("Interpolated transforms\n"); else diff --git a/Demos/FractureDemo/btFractureDynamicsWorld.cpp b/Demos/FractureDemo/btFractureDynamicsWorld.cpp index a5c13e83d..5df492d1b 100644 --- a/Demos/FractureDemo/btFractureDynamicsWorld.cpp +++ b/Demos/FractureDemo/btFractureDynamicsWorld.cpp @@ -516,7 +516,7 @@ void btFractureDynamicsWorld::fractureCallback( ) int j=f0; btCollisionObject* colOb = (btCollisionObject*)manifold->getBody1(); - btRigidBody* otherOb = btRigidBody::upcast(colOb); + // btRigidBody* otherOb = btRigidBody::upcast(colOb); // if (!otherOb->getInvMass()) // continue; diff --git a/Demos/OpenCLClothDemo/cl_cloth_demo.cpp b/Demos/OpenCLClothDemo/cl_cloth_demo.cpp index 5a48709b7..903127911 100644 --- a/Demos/OpenCLClothDemo/cl_cloth_demo.cpp +++ b/Demos/OpenCLClothDemo/cl_cloth_demo.cpp @@ -164,7 +164,7 @@ btSoftBody *createFromIndexedMesh( btVector3 *vertexArray, int numVertices, int // and can add a link across the link btAlignedObjectArray triangleForLinks; triangleForLinks.resize( numVertices * numVertices, -1 ); - int numLinks = 0; +// int numLinks = 0; for( int triangle = 0; triangle < numTriangles; ++triangle ) { int index[3] = {triangleVertexIndexArray[triangle * 3], triangleVertexIndexArray[triangle * 3 + 1], triangleVertexIndexArray[triangle * 3 + 2]}; @@ -449,7 +449,7 @@ void initBullet(void) #else capsuleTransform.setOrigin(btVector3(0, 0, 0)); - const btScalar pi = 3.141592654; + // const btScalar pi = 3.141592654; //capsuleTransform.setRotation(btQuaternion(0, 0, pi/2)); capsuleTransform.setRotation(btQuaternion(0, 0, 0)); #endif @@ -545,7 +545,8 @@ void doFlags() for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex ) { - g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor ); + if (g_softBodyOutput) + g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor ); cloths[flagIndex].draw(); } } diff --git a/Demos/OpenCLClothDemo/gl_win.cpp b/Demos/OpenCLClothDemo/gl_win.cpp index b1143ff71..464fd0e52 100644 --- a/Demos/OpenCLClothDemo/gl_win.cpp +++ b/Demos/OpenCLClothDemo/gl_win.cpp @@ -29,7 +29,7 @@ subject to the following restrictions: -static GLuint vbo = 0; +//static GLuint vbo = 0; #ifdef _WIN32 #include diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index a1d40e879..bddd135be 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -67,20 +67,7 @@ void GLDebugDrawer::drawSphere (const btVector3& p, btScalar radius, const btVec glPopMatrix(); } -void GLDebugDrawer::drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha) -{ - btVector3 halfExtent = (boxMax - boxMin) * btScalar(0.5f); - btVector3 center = (boxMax + boxMin) * btScalar(0.5f); - //glEnable(GL_BLEND); // Turn blending On - //glBlendFunc(GL_SRC_ALPHA, GL_ONE); - glColor4f (color.getX(), color.getY(), color.getZ(), alpha); - glPushMatrix (); - glTranslatef (center.getX(), center.getY(), center.getZ()); - glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]); -// glutSolidCube(1.0); - glPopMatrix (); - //glDisable(GL_BLEND); -} + void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) { diff --git a/Demos/OpenGL/GLDebugDrawer.h b/Demos/OpenGL/GLDebugDrawer.h index 4ac170337..6ac987d0b 100644 --- a/Demos/OpenGL/GLDebugDrawer.h +++ b/Demos/OpenGL/GLDebugDrawer.h @@ -19,7 +19,6 @@ public: virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color); virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color); - virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha); virtual void drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha); diff --git a/Demos/SharedOpenCL/btOpenCLUtils.cpp b/Demos/SharedOpenCL/btOpenCLUtils.cpp index 03fcf005e..a76971a09 100644 --- a/Demos/SharedOpenCL/btOpenCLUtils.cpp +++ b/Demos/SharedOpenCL/btOpenCLUtils.cpp @@ -32,7 +32,7 @@ subject to the following restrictions: #define btAssert assert //Set the preferred platform vendor using the OpenCL SDK -static char* spPlatformVendor = +static const char* spPlatformVendor = #if defined(CL_PLATFORM_MINI_CL) "MiniCL, SCEA"; #elif defined(CL_PLATFORM_AMD) @@ -312,9 +312,9 @@ void btOpenCLUtils::printDeviceInfo(cl_device_id device) printf(" CL_DEVICE_TYPE:\t\t\t%s\n", "CL_DEVICE_TYPE_DEFAULT"); printf(" CL_DEVICE_MAX_COMPUTE_UNITS:\t\t%u\n", info.m_computeUnits); - printf(" CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS:\t%u\n", info.m_workitemDims); - printf(" CL_DEVICE_MAX_WORK_ITEM_SIZES:\t%u / %u / %u \n", info.m_workItemSize[0], info.m_workItemSize[1], info.m_workItemSize[2]); - printf(" CL_DEVICE_MAX_WORK_GROUP_SIZE:\t%u\n", info.m_workgroupSize); + printf(" CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS:\t%zu\n", info.m_workitemDims); + printf(" CL_DEVICE_MAX_WORK_ITEM_SIZES:\t%zu / %zu / %zu \n", info.m_workItemSize[0], info.m_workItemSize[1], info.m_workItemSize[2]); + printf(" CL_DEVICE_MAX_WORK_GROUP_SIZE:\t%zu\n", info.m_workgroupSize); printf(" CL_DEVICE_MAX_CLOCK_FREQUENCY:\t%u MHz\n", info.m_clockFrequency); printf(" CL_DEVICE_ADDRESS_BITS:\t\t%u\n", info.m_addressBits); printf(" CL_DEVICE_MAX_MEM_ALLOC_SIZE:\t\t%u MByte\n", (unsigned int)(info.m_maxMemAllocSize/ (1024 * 1024))); @@ -333,11 +333,11 @@ void btOpenCLUtils::printDeviceInfo(cl_device_id device) printf(" CL_DEVICE_MAX_READ_IMAGE_ARGS:\t%u\n", info.m_maxReadImageArgs); printf(" CL_DEVICE_MAX_WRITE_IMAGE_ARGS:\t%u\n", info.m_maxWriteImageArgs); printf("\n CL_DEVICE_IMAGE "); - printf("\t\t\t2D_MAX_WIDTH\t %u\n", info.m_image2dMaxWidth); - printf("\t\t\t\t\t2D_MAX_HEIGHT\t %u\n", info.m_image2dMaxHeight); - printf("\t\t\t\t\t3D_MAX_WIDTH\t %u\n", info.m_image3dMaxWidth); - printf("\t\t\t\t\t3D_MAX_HEIGHT\t %u\n", info.m_image3dMaxHeight); - printf("\t\t\t\t\t3D_MAX_DEPTH\t %u\n", info.m_image3dMaxDepth); + printf("\t\t\t2D_MAX_WIDTH\t %zu\n", info.m_image2dMaxWidth); + printf("\t\t\t\t\t2D_MAX_HEIGHT\t %zu\n", info.m_image2dMaxHeight); + printf("\t\t\t\t\t3D_MAX_WIDTH\t %zu\n", info.m_image3dMaxWidth); + printf("\t\t\t\t\t3D_MAX_HEIGHT\t %zu\n", info.m_image3dMaxHeight); + printf("\t\t\t\t\t3D_MAX_DEPTH\t %zu\n", info.m_image3dMaxDepth); if (info.m_deviceExtensions != 0) printf("\n CL_DEVICE_EXTENSIONS:%s\n",info.m_deviceExtensions); else diff --git a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp index 3714f4817..3ab347ec4 100644 --- a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp @@ -1090,7 +1090,7 @@ bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile { btGeneric6DofSpringConstraintData* dofData = (btGeneric6DofSpringConstraintData*)constraintData; - int sz = sizeof(btGeneric6DofSpringConstraintData); + // int sz = sizeof(btGeneric6DofSpringConstraintData); btGeneric6DofSpringConstraint* dof = 0; if (rbA && rbB) diff --git a/UnitTests/BulletUnitTests/CMakeLists.txt b/UnitTests/BulletUnitTests/CMakeLists.txt index 560a6420e..8f38f0589 100644 --- a/UnitTests/BulletUnitTests/CMakeLists.txt +++ b/UnitTests/BulletUnitTests/CMakeLists.txt @@ -8,11 +8,24 @@ INCLUDE_DIRECTORIES( ) LINK_LIBRARIES( - cppunit BulletMultiThreaded BulletDynamics BulletCollision LinearMath + cppunit + BulletMultiThreaded + BulletSoftBody + BulletDynamics + BulletCollision + LinearMath ) ADD_EXECUTABLE(AppBulletUnitTests - Main.cpp - TestBulletOnly.h - TestLinearMath.h -) \ No newline at end of file + Main.cpp + TestBulletOnly.h + TestLinearMath.h + TestCholeskyDecomposition.cpp + TestCholeskyDecomposition.h + TestPolarDecomposition.cpp + TestPolarDecomposition.h + btCholeskyDecomposition.cpp + btCholeskyDecomposition.h +) + + diff --git a/UnitTests/BulletUnitTests/Main.cpp b/UnitTests/BulletUnitTests/Main.cpp index 4a3fad335..1bc8999cc 100644 --- a/UnitTests/BulletUnitTests/Main.cpp +++ b/UnitTests/BulletUnitTests/Main.cpp @@ -7,9 +7,15 @@ #include "TestBulletOnly.h" #include "TestLinearMath.h" +#include "TestPolarDecomposition.h" +#include "TestCholeskyDecomposition.h" + + CPPUNIT_TEST_SUITE_REGISTRATION( TestLinearMath ); + CPPUNIT_TEST_SUITE_REGISTRATION( TestBulletOnly ); + CPPUNIT_TEST_SUITE_REGISTRATION( TestPolarDecomposition ); + CPPUNIT_TEST_SUITE_REGISTRATION( TestCholeskyDecomposition ); + -CPPUNIT_TEST_SUITE_REGISTRATION( TestLinearMath ); -CPPUNIT_TEST_SUITE_REGISTRATION( TestBulletOnly ); int main(int argc, char* argv[]) diff --git a/UnitTests/BulletUnitTests/TestCholeskyDecomposition.cpp b/UnitTests/BulletUnitTests/TestCholeskyDecomposition.cpp new file mode 100644 index 000000000..d927bef09 --- /dev/null +++ b/UnitTests/BulletUnitTests/TestCholeskyDecomposition.cpp @@ -0,0 +1,68 @@ +#include "TestCholeskyDecomposition.h" +#include "btCholeskyDecomposition.h" + +void TestCholeskyDecomposition::testZeroMatrix() +{ + const btMatrix3x3 A(0,0,0,0,0,0,0,0,0); + const int result = choleskyDecompose(A, L); + + // The zero matrix is not positive definite so the decomposition does not + // exist. + CPPUNIT_ASSERT(result != 0); +} + +void TestCholeskyDecomposition::testIdentityMatrix() +{ + const btMatrix3x3 A = I; + const int result = choleskyDecompose(A, L); + + // The identity is a special case where the result should also be the + // identity. + CPPUNIT_ASSERT(result == 0); + CPPUNIT_ASSERT(equal(L, I)); +} + +void TestCholeskyDecomposition::testPositiveDefiniteMatrix() +{ + const btMatrix3x3 M(3,0,0,1,2,0,3,2,1); + const btMatrix3x3 A = M * M.transpose(); + const int result = choleskyDecompose(A, L); + + // By construction, the resulting decomposition of A should be equal to M + CPPUNIT_ASSERT(result == 0); + CPPUNIT_ASSERT(equal(L, M)); + CPPUNIT_ASSERT(equal(A, L * L.transpose())); +} + +void TestCholeskyDecomposition::testPositiveSemiDefiniteMatrix() +{ + const btMatrix3x3 M(3,0,0,1,0,0,3,2,1); + const btMatrix3x3 A = M * M.transpose(); + const int result = choleskyDecompose(A, L); + + // The matrix is semi definite, i.e. one of the eigenvalues is zero, so the + // Cholesky decomposition does not exist. + CPPUNIT_ASSERT(result != 0); +} + +void TestCholeskyDecomposition::testNegativeDefiniteMatrix() +{ + const btMatrix3x3 M(3,0,0,1,2,0,3,2,1); + const btMatrix3x3 A = M * M.transpose() * (-1.0); + const int result = choleskyDecompose(A, L); + + // The matrix is negative definite, i.e. all of the eigenvalues are negative, + // so the Cholesky decomposition does not exist. + CPPUNIT_ASSERT(result != 0); +} + +bool TestCholeskyDecomposition::equal(const btMatrix3x3& A, const btMatrix3x3& B) const +{ + for (unsigned int i = 0; i < 3; ++i) + for (unsigned int j = 0; j < 3; ++j) + if (btFabs(A[i][j] - B[i][j]) > SIMD_EPSILON) + return false; + + return true; +} + diff --git a/UnitTests/BulletUnitTests/TestCholeskyDecomposition.h b/UnitTests/BulletUnitTests/TestCholeskyDecomposition.h new file mode 100644 index 000000000..6ada9764d --- /dev/null +++ b/UnitTests/BulletUnitTests/TestCholeskyDecomposition.h @@ -0,0 +1,52 @@ +#ifndef TESTCHOLESKYDECOMPOSITION_H +#define TESTCHOLESKYDECOMPOSITION_H + +#include +#include +#include + +class TestCholeskyDecomposition : public CppUnit::TestFixture +{ + public: + + void setUp() + { + I.setIdentity(); + } + + void tearDown() + { + } + + void testZeroMatrix(); + void testIdentityMatrix(); + void testPositiveDefiniteMatrix(); + void testPositiveSemiDefiniteMatrix(); + void testNegativeDefiniteMatrix(); + + CPPUNIT_TEST_SUITE(TestCholeskyDecomposition); + CPPUNIT_TEST(testZeroMatrix); + CPPUNIT_TEST(testIdentityMatrix); + CPPUNIT_TEST(testPositiveDefiniteMatrix); + CPPUNIT_TEST(testPositiveSemiDefiniteMatrix); + CPPUNIT_TEST(testNegativeDefiniteMatrix); + CPPUNIT_TEST_SUITE_END(); + + private: + /** + * Returns TRUE if the specified matrices are equal and FALSE otherwise. + * + * @param A - the first matrix to be tested. + * @param B - the second matrix to be tested. + * + * @return a boolean indicating whether the specified matrix is symmetric. + */ + bool equal(const btMatrix3x3& A, const btMatrix3x3& B) const; + + private: + btMatrix3x3 L; + btMatrix3x3 I; +}; + +#endif // TESTCHOLESKYDECOMPOSITION_H + diff --git a/UnitTests/BulletUnitTests/TestLinearMath.h b/UnitTests/BulletUnitTests/TestLinearMath.h index 9c5e86078..6221db431 100644 --- a/UnitTests/BulletUnitTests/TestLinearMath.h +++ b/UnitTests/BulletUnitTests/TestLinearMath.h @@ -106,9 +106,9 @@ public: vec.safeNormalize(); CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-6 ); - vec.setValue(1e-20,0,0); - vec.normalize(); - CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 ); + //vec.setValue(1e-20,0,0); + //vec.normalize(); + //CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 ); vec.setValue(1e19,0,0); vec.normalize(); diff --git a/UnitTests/BulletUnitTests/TestPolarDecomposition.cpp b/UnitTests/BulletUnitTests/TestPolarDecomposition.cpp new file mode 100644 index 000000000..c24ccc745 --- /dev/null +++ b/UnitTests/BulletUnitTests/TestPolarDecomposition.cpp @@ -0,0 +1,103 @@ +#include "TestPolarDecomposition.h" +#include "BulletSoftBody/btSoftBodyInternals.h" +#include "LinearMath/btPolarDecomposition.h" +#include "btCholeskyDecomposition.h" + +namespace +{ + const int MAX_ITERATIONS = btPolarDecomposition::DEFAULT_MAX_ITERATIONS; + const btScalar TOLERANCE = btPolarDecomposition::DEFAULT_TOLERANCE; +} + +void TestPolarDecomposition::testZeroMatrix() +{ + const btMatrix3x3 A(0,0,0,0,0,0,0,0,0); + const int iterations = PolarDecompose(A, U, H); + + // The decomposition should fail because the zero matrix is singular + CPPUNIT_ASSERT(iterations == MAX_ITERATIONS); +} + +void TestPolarDecomposition::testSingularMatrix() +{ + const btMatrix3x3 A(1,0,0,1,0,0,0,0,1); + + const int iterations = PolarDecompose(A, U, H); + + // The cdecomposition should fail because the matrix is singular. + CPPUNIT_ASSERT(iterations == MAX_ITERATIONS); +} + +void TestPolarDecomposition::testPoorlyConditionedMatrix() +{ + const btScalar e = btScalar(TOLERANCE); + const btMatrix3x3 A(1,0,0,1,e,0,0,0,1); + + const int iterations = PolarDecompose(A, U, H); + + // The decomposition should succeed, however, the matrix is poorly + // conditioned, i.e. as 'e' approaches zero, the matrix approaches a singular + // matrix (no inverse). + CPPUNIT_ASSERT(iterations < MAX_ITERATIONS); + CPPUNIT_ASSERT(equal(A, U * H)); + CPPUNIT_ASSERT(isOrthogonal(U)); + CPPUNIT_ASSERT(isSymmetric(H)); + CPPUNIT_ASSERT(isPositiveDefinite(H)); +} + +void TestPolarDecomposition::testIdentityMatrix() +{ + const btMatrix3x3 A = I; + const int iterations = PolarDecompose(A, U, H); + + // The identity is a special case. The decomposition should succeed and both + // the U and H matrices should be equal to the identity. + CPPUNIT_ASSERT(iterations < MAX_ITERATIONS); + CPPUNIT_ASSERT(equal(A, U * H)); + CPPUNIT_ASSERT(equal(U, I)); + CPPUNIT_ASSERT(equal(H, I)); +} + +void TestPolarDecomposition::testNonSingularMatrix() +{ + const btMatrix3x3 A(4, 6, 6, 9, 2, 0, 1, 6, 0); + const int iterations = PolarDecompose(A, U, H); + + // The decomposition should succeed so that A = U*H, where U is orthogonal and + // H is symmetric and positive definite. + CPPUNIT_ASSERT(iterations < MAX_ITERATIONS); + CPPUNIT_ASSERT(equal(A, U * H)); + CPPUNIT_ASSERT(isOrthogonal(U)); + CPPUNIT_ASSERT(isSymmetric(H)); + CPPUNIT_ASSERT(isPositiveDefinite(H)); +} + +bool TestPolarDecomposition::equal(const btMatrix3x3& A, const btMatrix3x3& B) const +{ + for (unsigned int i = 0; i < 3; ++i) + for (unsigned int j = 0; j < 3; ++j) + if (btFabs(A[i][j] - B[i][j]) > TOLERANCE) + return false; + + return true; +} + +bool TestPolarDecomposition::isOrthogonal(const btMatrix3x3& A) const +{ + return equal(A.transpose() * A, I); +} + +bool TestPolarDecomposition::isPositiveDefinite(const btMatrix3x3& A) const +{ + static btMatrix3x3 storage; + return (0 == choleskyDecompose(A, storage)); +} + +bool TestPolarDecomposition::isSymmetric(const btMatrix3x3& A) const +{ + return + (btFabs(A[0][1] - A[1][0]) < TOLERANCE) && + (btFabs(A[0][2] - A[2][0]) < TOLERANCE) && + (btFabs(A[1][2] - A[2][1]) < TOLERANCE); +} + diff --git a/UnitTests/BulletUnitTests/TestPolarDecomposition.h b/UnitTests/BulletUnitTests/TestPolarDecomposition.h new file mode 100644 index 000000000..ed0ba9804 --- /dev/null +++ b/UnitTests/BulletUnitTests/TestPolarDecomposition.h @@ -0,0 +1,88 @@ +#ifndef TESTPOLARDECOMPOSITION_H +#define TESTPOLARDECOMPOSITION_H + +#include +#include +#include + +class TestPolarDecomposition : public CppUnit::TestFixture +{ + public: + + void setUp() + { + I.setIdentity(); + } + + void tearDown() + { + } + + void testPoorlyConditionedMatrix(); + void testSingularMatrix(); + void testNonSingularMatrix(); + void testIdentityMatrix(); + void testZeroMatrix(); + + CPPUNIT_TEST_SUITE(TestPolarDecomposition); + CPPUNIT_TEST(testZeroMatrix); + CPPUNIT_TEST(testSingularMatrix); + CPPUNIT_TEST(testPoorlyConditionedMatrix); + CPPUNIT_TEST(testIdentityMatrix); + CPPUNIT_TEST(testNonSingularMatrix); + CPPUNIT_TEST_SUITE_END(); + + private: + /** + * Returns TRUE if the specified matrix is orthogonal and FALSE otherwise. + * Orthogonality is checked by pre-multiplying the matrix by its transpose + * and comparing the result to the identity matrix. + * + * @param A - the matrix that is being tested + * + * @return a boolean indicating whether the specified matrix is orthogonal. + */ + bool isOrthogonal(const btMatrix3x3& A) const; + + /** + * Returns TRUE if the specified matrix is symmetric and FALSE otherwise. + * The symmetry of the matrix is determined by simplying testing the + * equality of the three off-diagonal elements. + * + * @param A - the matrix that is being tested. + * + * @return a boolean indicating whether the specified matrix is symmetric. + */ + bool isSymmetric(const btMatrix3x3& A) const; + + /** + * Returns TRUE if the specified matrix is positive definite and FALSE + * otherwise. The positive definiteness of the matrix is determined by + * calculating the Cholesky decomposition of the matrix; If the Cholesky + * decomposition exists, the original matrix is positive definite. + * + * @param A - the matrix that is being tested. + * + * @return a boolean indicating whether the specified matrix is positive + * definite. + */ + bool isPositiveDefinite(const btMatrix3x3& A) const; + + /** + * Returns TRUE if the specified matrices are equal and FALSE otherwise. + * + * @param A - the first matrix to be tested. + * @param B - the second matrix to be tested. + * + * @return a boolean indicating whether the specified matrix is symmetric. + */ + bool equal(const btMatrix3x3& A, const btMatrix3x3& B) const; + + private: + btMatrix3x3 U; + btMatrix3x3 H; + btMatrix3x3 I; +}; + +#endif // TESTPOLARDECOMPOSITION_H + diff --git a/UnitTests/BulletUnitTests/btCholeskyDecomposition.cpp b/UnitTests/BulletUnitTests/btCholeskyDecomposition.cpp new file mode 100644 index 000000000..4b3d65539 --- /dev/null +++ b/UnitTests/BulletUnitTests/btCholeskyDecomposition.cpp @@ -0,0 +1,40 @@ +#include "btCholeskyDecomposition.h" + +int choleskyDecompose(const btMatrix3x3& A, btMatrix3x3& L) +{ + // TODO Check that the A matrix is symmetric + + // Iterate over the elements of the upper triangle of A + for (unsigned int i = 0; i < 3; ++i) + { + for (unsigned int j = i; j < 3; ++j) + { + btScalar sum = A[i][j]; + for (unsigned int k = 0; k < i; ++k) + { + sum -= L[i][k] * L[j][k]; + } + + if (i != j) + { + L[j][i] = sum / L[i][i]; + } + else + { + if (sum <= btScalar(0)) + { + return btCholeskyDecomposition::FAILURE_POSITIVE_DEFINITE; + } + + L[i][i] = sqrt(sum); + } + } + } + + L[0][1] = btScalar(0); + L[0][2] = btScalar(0); + L[1][2] = btScalar(0); + + return btCholeskyDecomposition::SUCCESS; +} + diff --git a/UnitTests/BulletUnitTests/btCholeskyDecomposition.h b/UnitTests/BulletUnitTests/btCholeskyDecomposition.h new file mode 100644 index 000000000..7740ab820 --- /dev/null +++ b/UnitTests/BulletUnitTests/btCholeskyDecomposition.h @@ -0,0 +1,19 @@ +#ifndef BTCHOLESKYDECOMPOSITION_H +#define BTCHOLESKYDECOMPOSITION_H + +#include "LinearMath/btMatrix3x3.h" + +struct btCholeskyDecomposition +{ + enum Result + { + SUCCESS, + FAILURE_SYMMETRY, + FAILURE_POSITIVE_DEFINITE + }; +}; + +int choleskyDecompose(const btMatrix3x3& A, btMatrix3x3& L); + +#endif // BTCHOLESKYDECOMPOSITION_H + diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index a9050976b..4393dfeef 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -501,7 +501,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con } - void Process(int i) + void ProcessLeaf(int i) { const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); const btTransform& childTrans = m_compoundShape->getChildTransform(i); @@ -521,10 +521,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con my_cb); } - + void Process(const btDbvtNode* leaf) { - Process(leaf->dataAsInt); + ProcessLeaf(leaf->dataAsInt); } }; @@ -551,7 +551,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con { for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) { - rayCB.Process(i); + rayCB.ProcessLeaf(i); } } } diff --git a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index 6d426ba28..cb0f15218 100644 --- a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -40,10 +40,10 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - (void)body0; - (void)body1; + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + (void)body0Wrap; + (void)body1Wrap; void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); return new(mem) btEmptyAlgorithm(ci); } diff --git a/src/BulletCollision/Gimpact/btGImpactShape.h b/src/BulletCollision/Gimpact/btGImpactShape.h index 2a4f9386a..dbcd4d701 100644 --- a/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/src/BulletCollision/Gimpact/btGImpactShape.h @@ -639,7 +639,7 @@ public: { if(indicestype == PHY_SHORT) { - short * s_indices = (short *)(indexbase + face_index*indexstride); + unsigned short * s_indices = (unsigned short *)(indexbase + face_index*indexstride); i0 = s_indices[0]; i1 = s_indices[1]; i2 = s_indices[2]; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 31f845e5a..7090e8f4e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -449,6 +449,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { //body has already been converted solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); } else { btRigidBody* rb = btRigidBody::upcast(&body); @@ -1085,7 +1086,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j=0;jsolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + 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 @@ -1181,7 +1189,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { for (int j=0;jsolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + 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(); diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 9546b6213..f5330b51d 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -198,7 +198,7 @@ public: ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; + virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {}; const btRigidBody& getRigidBodyA() const diff --git a/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCL.cpp b/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCL.cpp index 3293c2b88..31a6f77f8 100644 --- a/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCL.cpp +++ b/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCL.cpp @@ -981,7 +981,7 @@ void btOpenCLSoftBodySolver::updateSoftBodies() int numVertices = m_vertexData.getNumVertices(); - int numTriangles = m_triangleData.getNumTriangles(); +// int numTriangles = m_triangleData.getNumTriangles(); // Ensure data is on accelerator m_vertexData.moveToAccelerator(); @@ -1240,8 +1240,8 @@ void btOpenCLSoftBodySolver::solveConstraints( float solverdt ) using Vectormath::Aos::dot; // Prepare links - int numLinks = m_linkData.getNumLinks(); - int numVertices = m_vertexData.getNumVertices(); +// int numLinks = m_linkData.getNumLinks(); +// int numVertices = m_vertexData.getNumVertices(); float kst = 1.f; float ti = 0.f; diff --git a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp index b27988cf3..62cf4f0f5 100644 --- a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp +++ b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp @@ -34,7 +34,7 @@ btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollision } #ifndef __SPU__ -SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1) +SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObject* body0,const btCollisionObject* body1) :btCollisionAlgorithm(ci) #ifdef USE_SEPDISTANCE_UTIL ,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc()) diff --git a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h index 083e15f4f..14b0a9454 100644 --- a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h +++ b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h @@ -21,6 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "LinearMath/btTransformUtil.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" class btPersistentManifold; @@ -37,8 +38,8 @@ ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btColli float m_collisionMargin0; float m_collisionMargin1; - btCollisionObject* m_collisionObject0; - btCollisionObject* m_collisionObject1; + const btCollisionObject* m_collisionObject0; + const btCollisionObject* m_collisionObject1; @@ -50,7 +51,7 @@ public: virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObject* body0,const btCollisionObject* body1); #ifdef USE_SEPDISTANCE_UTIL btConvexSeparatingDistanceUtil m_sepDistance; #endif //USE_SEPDISTANCE_UTIL @@ -68,12 +69,12 @@ public: return m_manifoldPtr; } - btCollisionObject* getCollisionObject0() + const btCollisionObject* getCollisionObject0() { return m_collisionObject0; } - btCollisionObject* getCollisionObject1() + const btCollisionObject* getCollisionObject1() { return m_collisionObject1; } @@ -108,10 +109,10 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm)); - return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1); + return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); } }; diff --git a/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp b/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp index 1de694075..e1d0219d5 100644 --- a/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp +++ b/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp @@ -393,7 +393,7 @@ void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher) -void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback) +void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) { btSimpleBroadphase::rayTest(rayFrom, rayTo, rayCallback); for (int i=0; i <= m_LastLargeHandleIndex; i++) diff --git a/src/BulletMultiThreaded/btGpu3DGridBroadphase.h b/src/BulletMultiThreaded/btGpu3DGridBroadphase.h index 1d49a0557..1154a5fa6 100644 --- a/src/BulletMultiThreaded/btGpu3DGridBroadphase.h +++ b/src/BulletMultiThreaded/btGpu3DGridBroadphase.h @@ -103,7 +103,9 @@ public: virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback); + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); + + virtual void resetPool(btDispatcher* dispatcher); protected: diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp index 76fd81730..2e400de1b 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp +++ b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp @@ -1271,7 +1271,7 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds]; //init those - float compFric = obA->getFriction()*obB->getFriction();//@todo + // float compFric = obA->getFriction()*obB->getFriction();//@todo int idA = obA->getCompanionId(); int idB = obB->getCompanionId(); @@ -1291,7 +1291,6 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int } - numPosPoints = numPosPoints; totalPoints+=numPosPoints; pfxSetRigidBodyIdA(pair,idA); pfxSetRigidBodyIdB(pair,idB); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c4a9b6c41..6232ec5db 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -110,7 +110,7 @@ void btSoftBody::initDefaults() m_initialWorldTransform.setIdentity(); m_windVelocity = btVector3(0,0,0); - + m_restLengthScale = btScalar(1.0); } // @@ -505,6 +505,18 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde if ( 0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. + btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt; + btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); + btScalar v_len2 = n.m_v.length2(); + + if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) + { + btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); + btScalar v_len = n.m_v.length(); + fDrag *= 0.8*(v_len / del_v_by_fDrag_len); + } + n.m_f += fDrag; n.m_f += fLift; } @@ -535,8 +547,8 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde const btScalar dt = m_sst.sdt; const btScalar kLF = m_cfg.kLF; const btScalar kDG = m_cfg.kDG; - const btScalar kPR = m_cfg.kPR; - const btScalar kVC = m_cfg.kVC; +// const btScalar kPR = m_cfg.kPR; +// const btScalar kVC = m_cfg.kVC; const bool as_lift = kLF>0; const bool as_drag = kDG>0; const bool as_aero = as_lift || as_drag; @@ -586,6 +598,18 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde { if (f.m_n[j]->m_im>0) { + // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node. + btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt; + btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); + btScalar v_len2 = f.m_n[j]->m_v.length2(); + + if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) + { + btScalar del_v_by_fDrag_len = del_v_by_fDrag.length(); + btScalar v_len = f.m_n[j]->m_v.length(); + fDrag *= 0.8*(v_len / del_v_by_fDrag_len); + } + f.m_n[j]->m_f += fDrag; f.m_n[j]->m_f += fLift; } @@ -816,6 +840,27 @@ void btSoftBody::scale(const btVector3& scl) updateConstants(); } +// +btScalar btSoftBody::getRestLengthScale() +{ + return m_restLengthScale; +} + +// +void btSoftBody::setRestLengthScale(btScalar restLengthScale) +{ + for(int i=0, ni=m_links.size(); im_x-l.m_n[1]->m_x).length(); + l.m_c1 = l.m_rl*l.m_rl; + } +} + // btScalar btSoftBody::getVolume() const { @@ -1587,7 +1643,7 @@ bool btSoftBody::cutLink(int node0,int node1,btScalar position) { bool done=false; int i,ni; - const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; +// const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x; const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position); const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position); const btScalar m=1; @@ -2178,7 +2234,7 @@ bool btSoftBody::checkContact( const btCollisionObjectWrapper* colObjWrap, { btVector3 nrm; const btCollisionShape *shp = colObjWrap->getCollisionShape(); - const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); +// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); const btTransform &wtr = colObjWrap->getWorldTransform(); //todo: check which transform is needed here @@ -2307,51 +2363,93 @@ void btSoftBody::updatePose() } // -void btSoftBody::updateConstants() +void btSoftBody::updateArea(bool averageArea) { int i,ni; + /* Face area */ + for(i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + } + + /* Node area */ + + if (averageArea) + { + btAlignedObjectArray counts; + counts.resize(m_nodes.size(),0); + for(i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); + } + } + for(i=0,ni=m_nodes.size();i0) + m_nodes[i].m_area/=(btScalar)counts[i]; + else + m_nodes[i].m_area=0; + } + } + else + { + // initialize node area as zero + for(i=0,ni=m_nodes.size();im_area += f.m_ra; + } + } + + for(i=0,ni=m_nodes.size();im_x-l.m_n[1]->m_x).length(); l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; - l.m_c1 = l.m_rl*l.m_rl; - } - /* Faces */ - for(i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); - } - /* Area's */ - btAlignedObjectArray counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); - } - } - for(i=0,ni=m_nodes.size();i0) - m_nodes[i].m_area/=(btScalar)counts[i]; - else - m_nodes[i].m_area=0; } } +void btSoftBody::updateConstants() +{ + resetLinkRestLengths(); + updateLinkConstants(); + updateArea(); +} + + + // void btSoftBody::initializeClusters() { @@ -2820,7 +2918,7 @@ void btSoftBody::applyForces() { BT_PROFILE("SoftBody applyForces"); - const btScalar dt = m_sst.sdt; +// const btScalar dt = m_sst.sdt; const btScalar kLF = m_cfg.kLF; const btScalar kDG = m_cfg.kDG; const btScalar kPR = m_cfg.kPR; @@ -2831,10 +2929,10 @@ void btSoftBody::applyForces() const bool as_volume = kVC>0; const bool as_aero = as_lift || as_drag ; - const bool as_vaero = as_aero && - (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); - const bool as_faero = as_aero && - (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); + //const bool as_vaero = as_aero && + // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); + //const bool as_faero = as_aero && + // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); const bool use_medium = as_aero; const bool use_volume = as_pressure || as_volume ; @@ -2877,7 +2975,7 @@ void btSoftBody::applyForces() /* Per face forces */ for(i=0,ni=m_faces.size();im_cfg.collisions&fCollision::CL_SELF) { btSoftColliders::CollideCL_SS docollide; - docollide.Process(this,psb); + docollide.ProcessSoftSoft(this,psb); } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 981c83240..2116c34f0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -671,6 +671,9 @@ public: btTransform m_initialWorldTransform; btVector3 m_windVelocity; + + btScalar m_restLengthScale; + // // Api // @@ -810,9 +813,15 @@ public: void rotate( const btQuaternion& rot); /* Scale */ void scale( const btVector3& scl); + /* Get link resting lengths scale */ + btScalar getRestLengthScale(); + /* Scale resting length of all springs */ + void setRestLengthScale(btScalar restLength); /* Set current state as pose */ void setPose( bool bvolume, bool bframe); + /* Set current link lengths as resting lengths */ + void resetLinkRestLengths(); /* Return the volume */ btScalar getVolume() const; /* Cluster count */ @@ -954,6 +963,8 @@ public: void updateBounds(); void updatePose(); void updateConstants(); + void updateLinkConstants(); + void updateArea(bool averageArea = true); void initializeClusters(); void updateClusters(); void cleanupClusters(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index dd5fd2434..19d0543ef 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -21,6 +21,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" +#include "LinearMath/btPolarDecomposition.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" @@ -614,32 +615,8 @@ private: // static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) { - static const btScalar half=(btScalar)0.5; - static const btScalar accuracy=(btScalar)0.0001; - static const int maxiterations=16; - int i=0; - btScalar det=0; - q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length()); - det = q.determinant(); - if(!btFuzzyZero(det)) - { - for(;iaccuracy) det=ndet; else break; - } - /* Final orthogonalization */ - Orthogonalize(q); - /* Compute 'S' */ - s=q.transpose()*m; - } - else - { - q.setIdentity(); - s.setIdentity(); - } - return(i); + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); } // @@ -753,7 +730,7 @@ struct btSoftColliders } } } - void Process(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) + void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) { psb = ps; m_colObjWrap = colObWrap; @@ -815,7 +792,7 @@ struct btSoftColliders } } - void Process(btSoftBody* psa,btSoftBody* psb) + void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) { idt = psa->m_sst.isdt; //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index e3696cd8a..01c148a2c 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -58,8 +58,8 @@ void btSoftRigidCollisionAlgorithm::processCollision (const btCollisionObjectWra (void)dispatchInfo; (void)resultOut; //printf("btSoftRigidCollisionAlgorithm\n"); - const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap; - const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap; +// const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap; +// const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap; btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject(); const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped? body0Wrap : body1Wrap; diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index 7a5fc445e..cc77583a0 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -8,6 +8,7 @@ SET(LinearMath_SRCS btConvexHull.cpp btConvexHullComputer.cpp btGeometryUtil.cpp + btPolarDecomposition.cpp btQuickprof.cpp btSerializer.cpp btVector3.cpp @@ -28,6 +29,7 @@ SET(LinearMath_HDRS btMatrix3x3.h btMinMax.h btMotionState.h + btPolarDecomposition.h btPoolAllocator.h btQuadWord.h btQuaternion.h diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index 935502f84..a00d7763a 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -280,6 +280,7 @@ class btIDebugDraw } } + virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) { drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); diff --git a/src/LinearMath/btPolarDecomposition.cpp b/src/LinearMath/btPolarDecomposition.cpp new file mode 100644 index 000000000..d7de20408 --- /dev/null +++ b/src/LinearMath/btPolarDecomposition.cpp @@ -0,0 +1,99 @@ +#include "btPolarDecomposition.h" +#include "btMinMax.h" + +namespace +{ + btScalar abs_column_sum(const btMatrix3x3& a, int i) + { + return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]); + } + + btScalar abs_row_sum(const btMatrix3x3& a, int i) + { + return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]); + } + + btScalar p1_norm(const btMatrix3x3& a) + { + const btScalar sum0 = abs_column_sum(a,0); + const btScalar sum1 = abs_column_sum(a,1); + const btScalar sum2 = abs_column_sum(a,2); + return btMax(btMax(sum0, sum1), sum2); + } + + btScalar pinf_norm(const btMatrix3x3& a) + { + const btScalar sum0 = abs_row_sum(a,0); + const btScalar sum1 = abs_row_sum(a,1); + const btScalar sum2 = abs_row_sum(a,2); + return btMax(btMax(sum0, sum1), sum2); + } +} + +const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001); +const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16; + +btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations) +: m_tolerance(tolerance) +, m_maxIterations(maxIterations) +{ +} + +unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const +{ + // Use the 'u' and 'h' matrices for intermediate calculations + u = a; + h = a.inverse(); + + for (unsigned int i = 0; i < m_maxIterations; ++i) + { + const btScalar h_1 = p1_norm(h); + const btScalar h_inf = pinf_norm(h); + const btScalar u_1 = p1_norm(u); + const btScalar u_inf = pinf_norm(u); + + const btScalar h_norm = h_1 * h_inf; + const btScalar u_norm = u_1 * u_inf; + + // The matrix is effectively singular so we cannot invert it + if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm)) + break; + + const btScalar gamma = btPow(h_norm / u_norm, 0.25f); + const btScalar inv_gamma = 1.0 / gamma; + + // Determine the delta to 'u' + const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5; + + // Update the matrices + u += delta; + h = u.inverse(); + + // Check for convergence + if (p1_norm(delta) <= m_tolerance * u_1) + { + h = u.transpose() * a; + h = (h + h.transpose()) * 0.5; + return i; + } + } + + // The algorithm has failed to converge to the specified tolerance, but we + // want to make sure that the matrices returned are in the right form. + h = u.transpose() * a; + h = (h + h.transpose()) * 0.5; + + return m_maxIterations; +} + +unsigned int btPolarDecomposition::maxIterations() const +{ + return m_maxIterations; +} + +unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) +{ + static btPolarDecomposition polar; + return polar.decompose(a, u, h); +} + diff --git a/src/LinearMath/btPolarDecomposition.h b/src/LinearMath/btPolarDecomposition.h new file mode 100644 index 000000000..561566764 --- /dev/null +++ b/src/LinearMath/btPolarDecomposition.h @@ -0,0 +1,73 @@ +#ifndef POLARDECOMPOSITION_H +#define POLARDECOMPOSITION_H + +#include "btMatrix3x3.h" + +/** + * This class is used to compute the polar decomposition of a matrix. In + * general, the polar decomposition factorizes a matrix, A, into two parts: a + * unitary matrix (U) and a positive, semi-definite Hermitian matrix (H). + * However, in this particular implementation the original matrix, A, is + * required to be a square 3x3 matrix with real elements. This means that U will + * be an orthogonal matrix and H with be a positive-definite, symmetric matrix. + */ +class btPolarDecomposition +{ + public: + static const btScalar DEFAULT_TOLERANCE; + static const unsigned int DEFAULT_MAX_ITERATIONS; + + /** + * Creates an instance with optional parameters. + * + * @param tolerance - the tolerance used to determine convergence of the + * algorithm + * @param maxIterations - the maximum number of iterations used to achieve + * convergence + */ + btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE, + unsigned int maxIterations = DEFAULT_MAX_ITERATIONS); + + /** + * Decomposes a matrix into orthogonal and symmetric, positive-definite + * parts. If the number of iterations returned by this function is equal to + * the maximum number of iterations, the algorithm has failed to converge. + * + * @param a - the original matrix + * @param u - the resulting orthogonal matrix + * @param h - the resulting symmetric matrix + * + * @return the number of iterations performed by the algorithm. + */ + unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const; + + /** + * Returns the maximum number of iterations that this algorithm will perform + * to achieve convergence. + * + * @return maximum number of iterations + */ + unsigned int maxIterations() const; + + private: + btScalar m_tolerance; + unsigned int m_maxIterations; +}; + +/** + * This functions decomposes the matrix 'a' into two parts: an orthogonal matrix + * 'u' and a symmetric, positive-definite matrix 'h'. If the number of + * iterations returned by this function is equal to + * btPolarDecomposition::DEFAULT_MAX_ITERATIONS, the algorithm has failed to + * converge. + * + * @param a - the original matrix + * @param u - the resulting orthogonal matrix + * @param h - the resulting symmetric matrix + * + * @return the number of iterations performed by the algorithm. + */ +unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h); + +#endif // POLARDECOMPOSITION_H + diff --git a/src/Makefile.am b/src/Makefile.am index be736bacd..aaa43aa85 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -95,6 +95,7 @@ libLinearMath_la_SOURCES = \ LinearMath/btAlignedAllocator.cpp \ LinearMath/btSerializer.cpp \ LinearMath/btConvexHull.cpp \ + LinearMath/btPolarDecomposition.cpp \ LinearMath/btVector3.cpp \ LinearMath/btConvexHullComputer.cpp \ LinearMath/btHashMap.h \ @@ -103,6 +104,7 @@ libLinearMath_la_SOURCES = \ LinearMath/btGeometryUtil.h \ LinearMath/btQuadWord.h \ LinearMath/btPoolAllocator.h \ + LinearMath/btPolarDecomposition.h \ LinearMath/btScalar.h \ LinearMath/btMinMax.h \ LinearMath/btVector3.h \ @@ -538,6 +540,7 @@ nobase_bullet_include_HEADERS += \ LinearMath/btMatrix3x3.h \ LinearMath/btVector3.h \ LinearMath/btPoolAllocator.h \ + LinearMath/btPolarDecomposition.h \ LinearMath/btScalar.h \ LinearMath/btDefaultMotionState.h \ LinearMath/btTransform.h \ diff --git a/src/MiniCL/MiniCL.cpp b/src/MiniCL/MiniCL.cpp index 04e75f5c2..52148dde0 100644 --- a/src/MiniCL/MiniCL.cpp +++ b/src/MiniCL/MiniCL.cpp @@ -128,7 +128,7 @@ CL_API_ENTRY cl_int CL_API_CALL clGetDeviceInfo( sprintf((char*)param_value,"%s",cpuName); } else { - printf("error: param_value_size should be at least %d, but it is %d\n",nameLen,param_value_size); + printf("error: param_value_size should be at least %d, but it is %zu\n",nameLen,param_value_size); return CL_INVALID_VALUE; } break; @@ -141,7 +141,7 @@ CL_API_ENTRY cl_int CL_API_CALL clGetDeviceInfo( *deviceType = CL_DEVICE_TYPE_CPU; } else { - printf("error: param_value_size should be at least %d\n",sizeof(cl_device_type)); + printf("error: param_value_size should be at least %zu\n",sizeof(cl_device_type)); return CL_INVALID_VALUE; } break; @@ -154,7 +154,7 @@ CL_API_ENTRY cl_int CL_API_CALL clGetDeviceInfo( *numUnits= 4; } else { - printf("error: param_value_size should be at least %d\n",sizeof(cl_uint)); + printf("error: param_value_size should be at least %zu\n",sizeof(cl_uint)); return CL_INVALID_VALUE; } @@ -172,7 +172,7 @@ CL_API_ENTRY cl_int CL_API_CALL clGetDeviceInfo( workItemSize[2] = 16; } else { - printf("error: param_value_size should be at least %d\n",sizeof(cl_uint)); + printf("error: param_value_size should be at least %zu\n",sizeof(cl_uint)); return CL_INVALID_VALUE; } break; diff --git a/src/MiniCL/cl_MiniCL_Defs.h b/src/MiniCL/cl_MiniCL_Defs.h index 73fd3c7d7..0773c8575 100644 --- a/src/MiniCL/cl_MiniCL_Defs.h +++ b/src/MiniCL/cl_MiniCL_Defs.h @@ -28,7 +28,7 @@ subject to the following restrictions: #define get_local_size(a) (gMiniCLNumOutstandingTasks) #define get_group_id(a) ((__guid_arg) / gMiniCLNumOutstandingTasks) -static unsigned int as_uint(float val) { return *((unsigned int*)&val); } +//static unsigned int as_uint(float val) { return *((unsigned int*)&val); } #define CLK_LOCAL_MEM_FENCE 0x01