diff options
author | Erwin Coumans <erwin.coumans@gmail.com> | 2019-08-14 21:14:56 -0700 |
---|---|---|
committer | Erwin Coumans <erwin.coumans@gmail.com> | 2019-08-14 21:14:56 -0700 |
commit | f09cefabe8712678dd8fd60c6a404b72aa27c6c4 (patch) | |
tree | aa4e6c81d648b8114410f4e055b0dc8c572c071c | |
parent | 88d1788ee510dc7f5aa92f67235667d342147e73 (diff) | |
parent | 6feb1b25db3d3730721fbb7ce8d600f973e0e174 (diff) | |
download | bullet3-f09cefabe8712678dd8fd60c6a404b72aa27c6c4.tar.gz |
Merge remote-tracking branch 'bp/master'
54 files changed, 5598 insertions, 843 deletions
diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp index b6a638c7f..8d3db3955 100644 --- a/Extras/obj2sdf/obj2sdf.cpp +++ b/Extras/obj2sdf/obj2sdf.cpp @@ -77,9 +77,10 @@ int main(int argc, char* argv[]) b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN); std::vector<tinyobj::shape_t> shapes; + tinyobj::attrib_t attribute; b3BulletDefaultFileIO fileIO; - std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath,&fileIO); + std::string err = tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath,&fileIO); char sdfFileName[MAX_PATH_LEN]; sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf"); @@ -117,20 +118,20 @@ int main(int argc, char* argv[]) int curTexcoords = shapeC->texcoords.size() / 2; int faceCount = shape.mesh.indices.size(); - int vertexCount = shape.mesh.positions.size(); + int vertexCount = attribute.vertices.size(); for (int v = 0; v < vertexCount; v++) { - shapeC->positions.push_back(shape.mesh.positions[v]); + shapeC->positions.push_back(attribute.vertices[v]); } - int numNormals = int(shape.mesh.normals.size()); + int numNormals = int(attribute.normals.size()); for (int vn = 0; vn < numNormals; vn++) { - shapeC->normals.push_back(shape.mesh.normals[vn]); + shapeC->normals.push_back(attribute.normals[vn]); } - int numTexCoords = int(shape.mesh.texcoords.size()); + int numTexCoords = int(attribute.texcoords.size()); for (int vt = 0; vt < numTexCoords; vt++) { - shapeC->texcoords.push_back(shape.mesh.texcoords[vt]); + shapeC->texcoords.push_back(attribute.texcoords[vt]); } for (int face = 0; face < faceCount; face += 3) @@ -140,9 +141,9 @@ int main(int argc, char* argv[]) continue; } - shapeC->indices.push_back(shape.mesh.indices[face] + curPositions); - shapeC->indices.push_back(shape.mesh.indices[face + 1] + curPositions); - shapeC->indices.push_back(shape.mesh.indices[face + 2] + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face].vertex_index + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face + 1].vertex_index + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face + 2].vertex_index + curPositions); } } } @@ -329,7 +330,7 @@ int main(int argc, char* argv[]) } int faceCount = shape.mesh.indices.size(); - int vertexCount = shape.mesh.positions.size(); + int vertexCount = attribute.vertices.size(); tinyobj::material_t mat = shape.material; if (shape.name.length()) { @@ -339,7 +340,7 @@ int main(int argc, char* argv[]) } for (int v = 0; v < vertexCount / 3; v++) { - fprintf(f, "v %f %f %f\n", shape.mesh.positions[v * 3 + 0], shape.mesh.positions[v * 3 + 1], shape.mesh.positions[v * 3 + 2]); + fprintf(f, "v %f %f %f\n", attribute.vertices[v * 3 + 0], attribute.vertices[v * 3 + 1], attribute.vertices[v * 3 + 2]); } if (mat.name.length()) @@ -352,18 +353,18 @@ int main(int argc, char* argv[]) } fprintf(f, "\n"); - int numNormals = int(shape.mesh.normals.size()); + int numNormals = int(attribute.normals.size()); for (int vn = 0; vn < numNormals / 3; vn++) { - fprintf(f, "vn %f %f %f\n", shape.mesh.normals[vn * 3 + 0], shape.mesh.normals[vn * 3 + 1], shape.mesh.normals[vn * 3 + 2]); + fprintf(f, "vn %f %f %f\n", attribute.normals[vn * 3 + 0], attribute.normals[vn * 3 + 1], attribute.normals[vn * 3 + 2]); } fprintf(f, "\n"); - int numTexCoords = int(shape.mesh.texcoords.size()); + int numTexCoords = int(attribute.texcoords.size()); for (int vt = 0; vt < numTexCoords / 2; vt++) { - fprintf(f, "vt %f %f\n", shape.mesh.texcoords[vt * 2 + 0], shape.mesh.texcoords[vt * 2 + 1]); + fprintf(f, "vt %f %f\n", attribute.texcoords[vt * 2 + 0], attribute.texcoords[vt * 2 + 1]); } fprintf(f, "s off\n"); @@ -375,9 +376,9 @@ int main(int argc, char* argv[]) continue; } fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n", - shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, - shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, - shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1); + shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1, + shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1, + shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1); } fclose(f); @@ -437,4 +438,4 @@ int main(int argc, char* argv[]) fclose(sdfFile); return 0; -}
\ No newline at end of file +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1918a9003..ba9efac19 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,4 +1,4 @@ -SUBDIRS( HelloWorld BasicDemo ) +SUBDIRS( HelloWorld BasicDemo) IF(BUILD_BULLET3) SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint ) ENDIF() diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp new file mode 100644 index 000000000..e947a5585 --- /dev/null +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -0,0 +1,409 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +///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 "DeformableMultibody.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" +#include "../SoftDemo/BunnyMesh.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../Utils/b3ResourcePath.h" +///The DeformableMultibody demo deformable bodies self-collision +static bool g_floatingBase = true; +static float friction = 1.; +class DeformableMultibody : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks; +public: + DeformableMultibody(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) + { + } + + virtual ~DeformableMultibody() + { + } + + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 30; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -10, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + virtual void stepSimulation(float deltaTime); + + btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false); + + void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); + + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonMultiBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableMultibody::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///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(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + btMultiBodyConstraintSolver* sol; + sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -40, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 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(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2); + } + + + { + bool damping = true; + bool gyro = false; + int numLinks = 4; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(.4, 1, .4); + btVector3 baseHalfExtents(.4, 1, .4); + + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if (!damping) + { + mbC->setLinearDamping(0.0f); + mbC->setAngularDamping(0.0f); + } + else + { + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); + } + + if (numLinks > 0) + { + btScalar q0 = 0.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents); + } + + // create a patch of cloth + { + btScalar h = 0; + const btScalar s = 4; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, +// 3,3, + 1 + 2 + 4 + 8, true); + + psb->getCollisionShape()->setMargin(0.25); + psb->generateBendingConstraints(2); + psb->setTotalMass(5); + psb->setSpringStiffness(2); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = .1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableMultibody::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; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +void DeformableMultibody::stepSimulation(float deltaTime) +{ +// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); + m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.); +} + + +btMultiBody* DeformableMultibody::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = .1f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + // pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = .1f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray<btQuaternion> world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray<btVector3> local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} +class CommonExampleInterface* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableMultibody(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableMultibody.h b/examples/DeformableDemo/DeformableMultibody.h new file mode 100644 index 000000000..acd9d421d --- /dev/null +++ b/examples/DeformableDemo/DeformableMultibody.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _DEFORMABLE_MULTIBODY_H +#define _DEFORMABLE_MULTIBODY_H + +class CommonExampleInterface* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_MULTIBODY_H diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp new file mode 100644 index 000000000..403ce4907 --- /dev/null +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -0,0 +1,291 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +///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 "DeformableRigid.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableRigid 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 DeformableRigid : public CommonRigidBodyBase +{ +public: + DeformableRigid(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableRigid() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void Ctor_RbUpStack(int count) + { + float mass = 0.2; + + btCompoundShape* cylinderCompound = new btCompoundShape; + btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); + btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5)); + btTransform localTransform; + localTransform.setIdentity(); + cylinderCompound->addChildShape(localTransform, boxShape); + btQuaternion orn(SIMD_HALF_PI, 0, 0); + localTransform.setRotation(orn); + // localTransform.setOrigin(btVector3(1,1,1)); + cylinderCompound->addChildShape(localTransform, cylinderShape); + + btCollisionShape* shape[] = { + new btBoxShape(btVector3(1, 1, 1)), +// new btSphereShape(0.75), +// cylinderCompound + }; +// static const int nshapes = sizeof(shape) / sizeof(shape[0]); +// for (int i = 0; i < count; ++i) +// { +// btTransform startTransform; +// startTransform.setIdentity(); +// startTransform.setOrigin(btVector3(0, 2+ 2 * i, 0)); +// startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); +// createRigidBody(mass, startTransform, shape[i % nshapes]); +// } + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(1, 1.5, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(1, 1.5, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 1.5, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 1.5, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(0, 3.5, 0)); + createRigidBody(mass, startTransform, shape[0]); + } + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableRigid::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///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(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + +// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 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); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + bool onGround = false; + const btScalar s = 4; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), +// 3,3, + 20,20, + 1 + 2 + 4 + 8, true); +// 0, true); + + if (onGround) + psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), +// 20,20, + 2,2, + 0, true); + + psb->getCollisionShape()->setMargin(0.1); + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->setSpringStiffness(1); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + + // add a few rigid bodies + Ctor_RbUpStack(1); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableRigid::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; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableRigid(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableRigid.h b/examples/DeformableDemo/DeformableRigid.h new file mode 100644 index 000000000..0d0e0dc5e --- /dev/null +++ b/examples/DeformableDemo/DeformableRigid.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _DEFORMABLE_RIGID_H +#define _DEFORMABLE_RIGID_H + +class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_RIGID_H diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp new file mode 100644 index 000000000..44e5f4e5f --- /dev/null +++ b/examples/DeformableDemo/Pinch.cpp @@ -0,0 +1,397 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +///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 "Pinch.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The Pinch 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). + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +struct TetraBunny +{ +#include "../SoftDemo/bunny.inl" +}; + + +class Pinch : public CommonRigidBodyBase +{ +public: + Pinch(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~Pinch() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 25; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createGrip() + { + int count = 2; + float mass = 1e6; + btCollisionShape* shape[] = { + new btBoxShape(btVector3(3, 3, 0.5)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(10, 0, 0)); + startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) +{ + btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies(); + if (rbs.size()<2) + return; + btRigidBody* rb0 = rbs[0]; + btScalar pressTime = 0.9; + btScalar liftTime = 2.5; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; + btTransform rbTransform; + rbTransform.setIdentity(); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 pinchVelocityLeft = btVector3(0,0,-2); + btVector3 pinchVelocityRight = btVector3(0,0,2); + btVector3 liftVelocity = btVector3(0,5,0); + btVector3 shiftVelocity = btVector3(0,0,5); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + + if (time < pressTime) + { + velocity = pinchVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb0->setCenterOfMassTransform(rbTransform); + rb0->setAngularVelocity(btVector3(0,0,0)); + rb0->setLinearVelocity(velocity); + + btRigidBody* rb1 = rbs[1]; + if (time < pressTime) + { + velocity = pinchVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb1->setCenterOfMassTransform(rbTransform); + rb1->setAngularVelocity(btVector3(0,0,0)); + rb1->setLinearVelocity(velocity); + + rb0->setFriction(20); + rb1->setFriction(20); +} + +void Pinch::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + getDeformableDynamicsWorld()->setSolverCallback(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + //create a ground + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -25, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 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(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a soft block + { + btScalar verts[24] = {0.f, 0.f, 0.f, + 1.f, 0.f, 0.f, + 0.f, 1.f, 0.f, + 0.f, 0.f, 1.f, + 1.f, 1.f, 0.f, + 0.f, 1.f, 1.f, + 1.f, 0.f, 1.f, + 1.f, 1.f, 1.f + }; + int triangles[60] = {0, 6, 3, + 0,1,6, + 7,5,3, + 7,3,6, + 4,7,6, + 4,6,1, + 7,2,5, + 7,4,2, + 0,3,2, + 2,3,5, + 0,2,4, + 0,4,1, + 0,6,5, + 0,6,4, + 3,4,2, + 3,4,7, + 2,7,3, + 2,7,1, + 4,5,0, + 4,5,6, + }; +// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20); +//// + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 4, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(1); +// psb->scale(btVector3(5, 5, 5)); +// psb->translate(btVector3(-2.5, 4, -2.5)); +// psb->getCollisionShape()->setMargin(0.1); +// psb->setTotalMass(1); + psb->setSpringStiffness(2); + psb->setDampingCoefficient(0.02); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + // add a grippers + createGrip(); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void Pinch::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; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options) +{ + return new Pinch(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/Pinch.h b/examples/DeformableDemo/Pinch.h new file mode 100644 index 000000000..1616ec39a --- /dev/null +++ b/examples/DeformableDemo/Pinch.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _PINCH_H +#define _PINCH_H + +class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options); + +#endif //_PINCH_H diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp new file mode 100644 index 000000000..74d9678a8 --- /dev/null +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -0,0 +1,296 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +///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 "VolumetricDeformable.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The VolumetricDeformable 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). + + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class VolumetricDeformable : public CommonRigidBodyBase +{ +public: + VolumetricDeformable(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~VolumetricDeformable() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createStaticBox(const btVector3& halfEdge, const btVector3& translation) + { + btCollisionShape* box = new btBoxShape(halfEdge); + m_collisionShapes.push_back(box); + + btTransform Transform; + Transform.setIdentity(); + Transform.setOrigin(translation); + Transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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) + box->calculateLocalInertia(mass, localInertia); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(Transform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, box, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + void Ctor_RbUpStack(int count) + { + float mass = 0.02; + + btCompoundShape* cylinderCompound = new btCompoundShape; + btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); + btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5)); + btTransform localTransform; + localTransform.setIdentity(); + cylinderCompound->addChildShape(localTransform, boxShape); + btQuaternion orn(SIMD_HALF_PI, 0, 0); + localTransform.setRotation(orn); + // localTransform.setOrigin(btVector3(1,1,1)); + cylinderCompound->addChildShape(localTransform, cylinderShape); + + btCollisionShape* shape[] = { + new btBoxShape(btVector3(1, 1, 1)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(i, 10 + 2 * i, i-1)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void VolumetricDeformable::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///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(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(50.), btScalar(150.))); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -50, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0)); + createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0)); + createStaticBox(btVector3(5, 5, 1), btVector3(0,0,5)); + createStaticBox(btVector3(5, 5, 1), btVector3(0,0,-5)); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 5, 0)); +// psb->setVolumeMass(10); + psb->getCollisionShape()->setMargin(0.25); +// psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->setSpringStiffness(1); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.5; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } + // add a few rigid bodies + Ctor_RbUpStack(4); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void VolumetricDeformable::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; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options) +{ + return new VolumetricDeformable(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/VolumetricDeformable.h b/examples/DeformableDemo/VolumetricDeformable.h new file mode 100644 index 000000000..fe5d326a5 --- /dev/null +++ b/examples/DeformableDemo/VolumetricDeformable.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _VOLUMETRIC_DEFORMABLE_H +#define _VOLUMETRIC_DEFORMABLE_H + +class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options); + +#endif //_VOLUMETRIC_DEFORMABLE__H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index a04e9f9b4..19334b2db 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -359,6 +359,14 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h + ../DeformableDemo/Pinch.cpp + ../DeformableDemo/Pinch.h + ../DeformableDemo/DeformableMultibody.cpp + ../DeformableDemo/DeformableMultibody.h + ../DeformableDemo/DeformableRigid.cpp + ../DeformableDemo/DeformableRigid.h + ../DeformableDemo/VolumetricDeformable.cpp + ../DeformableDemo/VolumetricDeformable.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e80b4eee9..481d36c5d 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -47,6 +47,10 @@ #include "../FractureDemo/FractureDemo.h" #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" +#include "../DeformableDemo/DeformableRigid.h" +#include "../DeformableDemo/Pinch.h" +#include "../DeformableDemo/DeformableMultibody.h" +#include "../DeformableDemo/VolumetricDeformable.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -117,7 +121,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Basic Example", "Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc), ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc), - + ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.", AllConstraintCreateFunc), @@ -190,6 +194,13 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc, TUT_SPHERE_PLANE_BULLET2), //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), + ExampleEntry(0, "Deformabe Body"), + ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), + ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), + ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), + // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), + #ifdef INCLUDE_CLOTH_DEMOS ExampleEntry(0, "Soft Body"), ExampleEntry(1, "Cloth", "Simulate a patch of cloth.", SoftDemoCreateFunc, 0), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 58b67a3bf..a0b8f1587 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -182,6 +182,7 @@ project "App_BulletExampleBrowser" "../RenderingExamples/*", "../VoronoiFracture/*", "../SoftDemo/*", + "../DeformableDemo/*", "../RollingFrictionDemo/*", "../rbdl/*", "../FractureDemo/*", diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index d630f8599..f868bcb13 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -2265,7 +2265,7 @@ int BulletMJCFImporter::getBodyUniqueId() const return m_data->m_activeBodyUniqueId; } -static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin) +static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin) { btCompoundShape* compound = new btCompoundShape(); compound->setMargin(collisionMargin); @@ -2278,25 +2278,26 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::sha btConvexHullShape* convexHull = new btConvexHullShape(); convexHull->setMargin(collisionMargin); tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); for (int f = 0; f < faceCount; f += 3) { btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); } @@ -2391,10 +2392,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn else { std::vector<tinyobj::shape_t> shapes; - std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape - childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); + childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); } break; } diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index b4e02243d..6f13d86b2 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -65,13 +65,14 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& btVector3 shift(0, 0, 0); std::vector<tinyobj::shape_t> shapes; + tinyobj::attrib_t attribute; { B3_PROFILE("tinyobj::LoadObj"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO); + std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO); //std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); } - GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); + GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes); { B3_PROFILE("Load Texture"); //int textureIndex = -1; @@ -84,7 +85,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_rgbaColor[2] = shape.material.diffuse[2]; meshData.m_rgbaColor[3] = shape.material.transparency; meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR; - + meshData.m_specularColor[0] = shape.material.specular[0]; meshData.m_specularColor[1] = shape.material.specular[1]; meshData.m_specularColor[2] = shape.material.specular[2]; diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index 566682d0a..e5bd6633c 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -12,6 +12,7 @@ struct CachedObjResult { std::string m_msg; std::vector<tinyobj::shape_t> m_shapes; + tinyobj::attrib_t m_attribute; }; static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults; @@ -31,24 +32,26 @@ void b3EnableFileCaching(int enable) } std::string LoadFromCachedOrFromObj( + tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, // [output] const char* filename, const char* mtl_basepath, - struct CommonFileIOInterface* fileIO - ) + struct CommonFileIOInterface* fileIO) { CachedObjResult* resultPtr = gCachedObjResults[filename]; if (resultPtr) { const CachedObjResult& result = *resultPtr; shapes = result.m_shapes; + attribute = result.m_attribute; return result.m_msg; } - std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO); + std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO); CachedObjResult result; result.m_msg = err; result.m_shapes = shapes; + result.m_attribute = attribute; if (gEnableFileCaching) { gCachedObjResults.insert(filename, result); @@ -60,14 +63,15 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const cha { B3_PROFILE("LoadMeshFromObj"); std::vector<tinyobj::shape_t> shapes; + tinyobj::attrib_t attribute; { B3_PROFILE("tinyobj::LoadObj2"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO); + std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO); } { B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj"); - GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); + GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes); return gfxShape; } } diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index 9b62074c1..54bc66312 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -8,8 +8,8 @@ struct GLInstanceGraphicsShape; int b3IsFileCachingEnabled(); void b3EnableFileCaching(int enable); - std::string LoadFromCachedOrFromObj( + tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, // [output] const char* filename, const char* mtl_basepath, diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp index ead2769a0..08fa521ae 100644 --- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp +++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp @@ -9,7 +9,7 @@ #include "../../OpenGLWindow/GLInstancingRenderer.h" #include "../../OpenGLWindow/GLInstanceGraphicsShape.h" -GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading) +GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading) { b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>; { @@ -36,19 +36,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny } GLInstanceVertex vtx0; - vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 0]; - vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 1]; - vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]; + tinyobj::index_t v_0 = shape.mesh.indices[f]; + vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index]; + vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1]; + vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2]; vtx0.xyzw[3] = 0.f; - if (shape.mesh.texcoords.size()) + if (attribute.texcoords.size()) { - int uv0Index = shape.mesh.indices[f] * 2 + 0; - int uv1Index = shape.mesh.indices[f] * 2 + 1; - if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))) + int uv0Index = 2 * v_0.texcoord_index; + int uv1Index = 2 * v_0.texcoord_index + 1; + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))) { - vtx0.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx0.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx0.uv[0] = attribute.texcoords[uv0Index]; + vtx0.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -64,19 +65,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny } GLInstanceVertex vtx1; - vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0]; - vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1]; - vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]; + tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; + vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index]; + vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1]; + vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2]; vtx1.xyzw[3] = 0.f; - if (shape.mesh.texcoords.size()) + if (attribute.texcoords.size()) { - int uv0Index = shape.mesh.indices[f + 1] * 2 + 0; - int uv1Index = shape.mesh.indices[f + 1] * 2 + 1; - if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())) + int uv0Index = 2 * v_1.texcoord_index; + int uv1Index = 2 * v_1.texcoord_index + 1; + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())) { - vtx1.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx1.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx1.uv[0] = attribute.texcoords[uv0Index]; + vtx1.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -92,18 +94,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny } GLInstanceVertex vtx2; - vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0]; - vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1]; - vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]; + tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; + vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index]; + vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1]; + vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2]; vtx2.xyzw[3] = 0.f; - if (shape.mesh.texcoords.size()) + if (attribute.texcoords.size()) { - int uv0Index = shape.mesh.indices[f + 2] * 2 + 0; - int uv1Index = shape.mesh.indices[f + 2] * 2 + 1; - if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())) + int uv0Index = 2 * v_2.texcoord_index; + int uv1Index = 2 * v_2.texcoord_index + 1; + + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())) { - vtx2.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx2.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx2.uv[0] = attribute.texcoords[uv0Index]; + vtx2.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -123,16 +127,21 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny btVector3 v2(vtx2.xyzw[0], vtx2.xyzw[1], vtx2.xyzw[2]); unsigned int maxIndex = 0; - maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 0); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 1); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 2); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 0); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 1); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 2); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 0); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 1); - maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 2); - bool hasNormals = (shape.mesh.normals.size() && maxIndex < shape.mesh.normals.size()); + unsigned n0Index = shape.mesh.indices[f].normal_index; + unsigned n1Index = shape.mesh.indices[f + 1].normal_index; + unsigned n2Index = shape.mesh.indices[f + 2].normal_index; + + maxIndex = b3Max(maxIndex, 3 * n0Index + 0); + maxIndex = b3Max(maxIndex, 3 * n0Index + 1); + maxIndex = b3Max(maxIndex, 3 * n0Index + 2); + maxIndex = b3Max(maxIndex, 3 * n1Index + 0); + maxIndex = b3Max(maxIndex, 3 * n1Index + 1); + maxIndex = b3Max(maxIndex, 3 * n1Index + 2); + maxIndex = b3Max(maxIndex, 3 * n2Index + 0); + maxIndex = b3Max(maxIndex, 3 * n2Index + 1); + maxIndex = b3Max(maxIndex, 3 * n2Index + 2); + + bool hasNormals = (attribute.normals.size() && maxIndex < attribute.normals.size()); if (flatShading || !hasNormals) { @@ -159,15 +168,15 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny } else { - vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 0]; - vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 1]; - vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 2]; //shape.mesh.indices[f+1]*3+0 - vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 0]; - vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 1]; - vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 2]; - vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 0]; - vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 1]; - vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 2]; + vtx0.normal[0] = attribute.normals[3 * n0Index+ 0]; + vtx0.normal[1] = attribute.normals[3 * n0Index+ 1]; + vtx0.normal[2] = attribute.normals[3 * n0Index+ 2]; + vtx1.normal[0] = attribute.normals[3 * n1Index+ 0]; + vtx1.normal[1] = attribute.normals[3 * n1Index+ 1]; + vtx1.normal[2] = attribute.normals[3 * n1Index+ 2]; + vtx2.normal[0] = attribute.normals[3 * n2Index+ 0]; + vtx2.normal[1] = attribute.normals[3 * n2Index+ 1]; + vtx2.normal[2] = attribute.normals[3 * n2Index+ 2]; } vertices->push_back(vtx0); vertices->push_back(vtx1); diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h index 2840a85d7..4054b4dab 100644 --- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h +++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h @@ -4,6 +4,6 @@ #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include <vector> -struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading = false); +struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading = false); #endif //WAVEFRONT2GRAPHICS_H diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index f295458a8..16c85d0b5 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -509,7 +509,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } -static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags) +static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags) { B3_PROFILE("createConvexHullFromShapes"); btCompoundShape* compound = new btCompoundShape(); @@ -528,20 +528,20 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t for (int f = 0; f < faceCount; f += 3) { btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); } @@ -558,8 +558,6 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t return compound; } - - int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const { UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape); @@ -718,10 +716,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl else { std::vector<tinyobj::shape_t> shapes; - std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape - - shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags); + shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags); m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision); return shape; } diff --git a/examples/MultiBodyBaseline/MultiBodyBaseline.cpp b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp new file mode 100644 index 000000000..663c2d33a --- /dev/null +++ b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp @@ -0,0 +1,358 @@ +/* +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 "MultiBodyBaseline.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" +#include "../SoftDemo/BunnyMesh.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../Utils/b3ResourcePath.h" +///The MultiBodyBaseline demo deformable bodies self-collision +static bool g_floatingBase = true; +static float friction = 1.; +class MultiBodyBaseline : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks; +public: + MultiBodyBaseline(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) + { + } + + virtual ~MultiBodyBaseline() + { + } + + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 30; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -10, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + virtual void stepSimulation(float deltaTime); + + btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false); + + void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); + +}; + +void MultiBodyBaseline::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///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(); + btMultiBodyConstraintSolver* sol; + sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration); + m_dynamicsWorld = world; + // m_dynamicsWorld->setDebugDrawer(&gDebugDraw); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -40, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 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(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2); + } + + + { + bool damping = true; + bool gyro = false; + int numLinks = 4; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(1, 1, 1); + btVector3 baseHalfExtents(1, 1, 1); + + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if (!damping) + { + mbC->setLinearDamping(0.0f); + mbC->setAngularDamping(0.0f); + } + else + { + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); + } + + if (numLinks > 0) + { + btScalar q0 = 0.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void MultiBodyBaseline::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; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +void MultiBodyBaseline::stepSimulation(float deltaTime) +{ +// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); + m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.); +} + + +btMultiBody* MultiBodyBaseline::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = .1f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + // pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = .1f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void MultiBodyBaseline::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray<btQuaternion> world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray<btVector3> local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} +class CommonExampleInterface* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options) +{ + return new MultiBodyBaseline(options.m_guiHelper); +} + + diff --git a/examples/MultiBodyBaseline/MultiBodyBaseline.h b/examples/MultiBodyBaseline/MultiBodyBaseline.h new file mode 100644 index 000000000..35734c63c --- /dev/null +++ b/examples/MultiBodyBaseline/MultiBodyBaseline.h @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef _MULTIBODY_BASELINE_H +#define _MULTIBODY_BASELINE_H + +class CommonExampleInterface* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options); + +#endif //_MULTIBODY_BASELINE_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f6b48a8ce..d9db25737 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4861,7 +4861,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str else { std::vector<tinyobj::shape_t> shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); //static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale) @@ -4882,20 +4883,20 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str for (int f = 0; f < faceCount; f += 3) { btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); } @@ -7958,8 +7959,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; bool hasStatus = true; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 0.1; - double mass = 0.1; + double scale = 1; + double mass = 1; double collisionMargin = 0.02; const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; if (m_data->m_verboseOutput) @@ -8012,23 +8013,24 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar std::string out_found_filename; int out_type; - bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); std::vector<tinyobj::shape_t> shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); if (!shapes.empty()) { const tinyobj::shape_t& shape = shapes[0]; btAlignedObjectArray<btScalar> vertices; btAlignedObjectArray<int> indices; - for (int i = 0; i < shape.mesh.positions.size(); i++) + for (int i = 0; i < attribute.vertices.size(); i++) { - vertices.push_back(shape.mesh.positions[i]); + vertices.push_back(attribute.vertices[i]); } for (int i = 0; i < shape.mesh.indices.size(); i++) { - indices.push_back(shape.mesh.indices[i]); + indices.push_back(shape.mesh.indices[i].vertex_index); } - int numTris = indices.size() / 3; + int numTris = shape.mesh.indices.size() / 3; if (numTris > 0) { btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); @@ -8041,9 +8043,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar //turn on softbody vs softbody collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); + psb->scale(btVector3(scale, scale, scale)); psb->rotate(initialOrn); psb->translate(initialPos); - psb->scale(btVector3(scale, scale, scale)); psb->setTotalMass(mass, true); psb->getCollisionShape()->setMargin(collisionMargin); diff --git a/examples/ThirdPartyLibs/Wavefront/README.md b/examples/ThirdPartyLibs/Wavefront/README.md index 6f911e523..de226acd3 100644 --- a/examples/ThirdPartyLibs/Wavefront/README.md +++ b/examples/ThirdPartyLibs/Wavefront/README.md @@ -38,51 +38,102 @@ Licensed under 2 clause BSD. Usage ----- +Data format +attrib_t contains single and linear array of vertex data(position, normal and texcoord). - std::string inputfile = "cornell_box.obj"; - std::vector<tinyobj::shape_t> shapes; - - std::string err = tinyobj::LoadObj(shapes, inputfile.c_str()); - - if (!err.empty()) { - std::cerr << err << std::endl; - exit(1); - } - - std::cout << "# of shapes : " << shapes.size() << std::endl; - - for (size_t i = 0; i < shapes.size(); i++) { - printf("shape[%ld].name = %s\n", i, shapes[i].name.c_str()); - printf("shape[%ld].indices: %ld\n", i, shapes[i].mesh.indices.size()); - assert((shapes[i].mesh.indices.size() % 3) == 0); - for (size_t f = 0; f < shapes[i].mesh.indices.size(); f++) { - printf(" idx[%ld] = %d\n", f, shapes[i].mesh.indices[f]); - } - - printf("shape[%ld].vertices: %ld\n", i, shapes[i].mesh.positions.size()); - assert((shapes[i].mesh.positions.size() % 3) == 0); - for (size_t v = 0; v < shapes[i].mesh.positions.size() / 3; v++) { - printf(" v[%ld] = (%f, %f, %f)\n", v, - shapes[i].mesh.positions[3*v+0], - shapes[i].mesh.positions[3*v+1], - shapes[i].mesh.positions[3*v+2]); - } - - printf("shape[%ld].material.name = %s\n", i, shapes[i].material.name.c_str()); - printf(" material.Ka = (%f, %f ,%f)\n", shapes[i].material.ambient[0], shapes[i].material.ambient[1], shapes[i].material.ambient[2]); - printf(" material.Kd = (%f, %f ,%f)\n", shapes[i].material.diffuse[0], shapes[i].material.diffuse[1], shapes[i].material.diffuse[2]); - printf(" material.Ks = (%f, %f ,%f)\n", shapes[i].material.specular[0], shapes[i].material.specular[1], shapes[i].material.specular[2]); - printf(" material.Tr = (%f, %f ,%f)\n", shapes[i].material.transmittance[0], shapes[i].material.transmittance[1], shapes[i].material.transmittance[2]); - printf(" material.Ke = (%f, %f ,%f)\n", shapes[i].material.emission[0], shapes[i].material.emission[1], shapes[i].material.emission[2]); - printf(" material.Ns = %f\n", shapes[i].material.shininess); - printf(" material.map_Ka = %s\n", shapes[i].material.ambient_texname.c_str()); - printf(" material.map_Kd = %s\n", shapes[i].material.diffuse_texname.c_str()); - printf(" material.map_Ks = %s\n", shapes[i].material.specular_texname.c_str()); - printf(" material.map_Ns = %s\n", shapes[i].material.normal_texname.c_str()); - std::map<std::string, std::string>::iterator it(shapes[i].material.unknown_parameter.begin()); - std::map<std::string, std::string>::iterator itEnd(shapes[i].material.unknown_parameter.end()); - for (; it != itEnd; it++) { - printf(" material.%s = %s\n", it->first.c_str(), it->second.c_str()); - } - printf("\n"); +attrib_t::vertices => 3 floats per vertex + + v[0] v[1] v[2] v[3] v[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::normals => 3 floats per vertex + + n[0] n[1] n[2] n[3] n[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::texcoords => 2 floats per vertex + + t[0] t[1] t[2] t[3] t[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | u | v | u | v | u | v | u | v | .... | u | v | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::colors => 3 floats per vertex(vertex color. optional) + + c[0] c[1] c[2] c[3] c[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +Each shape_t::mesh_t does not contain vertex data but contains array index to attrib_t. See loader_example.cc for more details. + + +mesh_t::indices => array of vertex indices. + + +----+----+----+----+----+----+----+----+----+----+ +--------+ + | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-1) | + +----+----+----+----+----+----+----+----+----+----+ +--------+ + +Each index has an array index to attrib_t::vertices, attrib_t::normals and attrib_t::texcoords. + +mesh_t::num_face_vertices => array of the number of vertices per face(e.g. 3 = triangle, 4 = quad , 5 or more = N-gons). + + + +---+---+---+ +---+ + | 3 | 4 | 3 | ...... | 3 | + +---+---+---+ +---+ + | | | | + | | | +-----------------------------------------+ + | | | | + | | +------------------------------+ | + | | | | + | +------------------+ | | + | | | | + |/ |/ |/ |/ + + mesh_t::indices + + | face[0] | face[1] | face[2] | | face[n-1] | + +----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+ + | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-3) | i(n-2) | i(n-1) | + +----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+ + +```c++ +#define TINYOBJLOADER_IMPLEMENTATION // define this in only *one* .cc +#include "tiny_obj_loader.h" + +std::string inputfile = "cornell_box.obj"; +tinyobj::attrib_t attrib; +std::vector<tinyobj::shape_t> shapes; + +LoadObj(attrib, shapes, inputfile.c_str()); + +// Loop over shapes +for (size_t s = 0; s < shapes.size(); s++) { + // Loop over faces(polygon) + size_t index_offset = 0; + for (size_t f = 0; f < shapes[s].mesh.indices.size(); f++) { + int fv = 3; + // Loop over vertices in the face. + for (size_t v = 0; v < fv; v++) { + // access to vertex + tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v]; + tinyobj::real_t vx = attrib.vertices[3*idx.vertex_index+0]; + tinyobj::real_t vy = attrib.vertices[3*idx.vertex_index+1]; + tinyobj::real_t vz = attrib.vertices[3*idx.vertex_index+2]; + tinyobj::real_t nx = attrib.normals[3*idx.normal_index+0]; + tinyobj::real_t ny = attrib.normals[3*idx.normal_index+1]; + tinyobj::real_t nz = attrib.normals[3*idx.normal_index+2]; + tinyobj::real_t tx = attrib.texcoords[2*idx.texcoord_index+0]; + tinyobj::real_t ty = attrib.texcoords[2*idx.texcoord_index+1]; } + index_offset += fv; + } +} + +``` + diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index 9cd2f2092..d1b9c6362 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -70,25 +70,6 @@ std::istream& safeGetline(std::istream& is, std::string& t) } } #endif -struct vertex_index -{ - int v_idx, vt_idx, vn_idx, dummy; -}; -struct MyIndices -{ - int m_offset; - int m_numIndices; -}; - -// for std::map -static inline bool operator<(const vertex_index& a, const vertex_index& b) -{ - if (a.v_idx != b.v_idx) return (a.v_idx < b.v_idx); - if (a.vn_idx != b.vn_idx) return (a.vn_idx < b.vn_idx); - if (a.vt_idx != b.vt_idx) return (a.vt_idx < b.vt_idx); - - return false; -} static inline bool isSpace(const char c) { @@ -101,25 +82,33 @@ static inline bool isNewLine(const char c) } // Make index zero-base, and also support relative index. -static inline int fixIndex(int idx, int n) +static inline bool fixIndex(int idx, int n, int* ret) { - int i; + if (!ret) + { + return false; + } if (idx > 0) { - i = idx - 1; + (*ret) = idx - 1; + return true; } - else if (idx == 0) + + if (idx == 0) { - i = 0; + // zero is not allowed according to the spec. + return false; } - else - { // negative value = relative - i = n + idx; + + if (idx < 0) + { + (*ret) = n + idx; // negative value = relative + return true; } - return i; -} + return false; // never reach here. +} static inline std::string parseString(const char*& token) { std::string s; @@ -156,170 +145,184 @@ static inline void parseFloat3( z = parseFloat(token); } -// Parse triples: i, i/j/k, i//k, i/j -static vertex_index parseTriple( - const char*& token, - int vsize, - int vnsize, - int vtsize) +// Parse triples with index offsets: i, i/j/k, i//k, i/j +static bool parseTriple(const char** token, int vsize, int vnsize, int vtsize, + vertex_index_t* ret) { - vertex_index vi; - vi.vn_idx = -1; - vi.vt_idx = -1; - vi.v_idx = -1; - - vi.v_idx = fixIndex(atoi(token), vsize); - token += strcspn(token, "/ \t\r"); - if (token[0] != '/') + if (!ret) { - return vi; + return false; } - token++; - // i//k - if (token[0] == '/') + vertex_index_t vi(-1); + + if (!fixIndex(atoi((*token)), vsize, &(vi.v_idx))) { - token++; - vi.vn_idx = fixIndex(atoi(token), vnsize); - token += strcspn(token, "/ \t\r"); - return vi; + return false; } - // i/j/k or i/j - vi.vt_idx = fixIndex(atoi(token), vtsize); - token += strcspn(token, "/ \t\r"); - if (token[0] != '/') + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { - return vi; + (*ret) = vi; + return true; } + (*token)++; - // i/j/k - token++; // skip '/' - vi.vn_idx = fixIndex(atoi(token), vnsize); - token += strcspn(token, "/ \t\r"); - return vi; -} - -static unsigned int -updateVertex( - std::map<vertex_index, unsigned int>& vertexCache, - std::vector<float>& positions, - std::vector<float>& normals, - std::vector<float>& texcoords, - const std::vector<float>& in_positions, - const std::vector<float>& in_normals, - const std::vector<float>& in_texcoords, - const vertex_index& i) -{ - const std::map<vertex_index, unsigned int>::iterator it = vertexCache.find(i); - - if (it != vertexCache.end()) + // i//k + if ((*token)[0] == '/') { - // found cache - return it->second; + (*token)++; + if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) + { + return false; + } + (*token) += strcspn((*token), "/ \t\r"); + (*ret) = vi; + return true; } - assert(static_cast<int>(in_positions.size()) > (3 * i.v_idx + 2)); - - positions.push_back(in_positions[3 * i.v_idx + 0]); - positions.push_back(in_positions[3 * i.v_idx + 1]); - positions.push_back(in_positions[3 * i.v_idx + 2]); - - if (i.vn_idx >= 0 && ((3 * i.vn_idx + 2) < in_normals.size())) + // i/j/k or i/j + if (!fixIndex(atoi((*token)), vtsize, &(vi.vt_idx))) { - normals.push_back(in_normals[3 * i.vn_idx + 0]); - normals.push_back(in_normals[3 * i.vn_idx + 1]); - normals.push_back(in_normals[3 * i.vn_idx + 2]); + return false; } - if (i.vt_idx >= 0) + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { - int numTexCoords = in_texcoords.size(); - int index0 = 2 * i.vt_idx + 0; - int index1 = 2 * i.vt_idx + 1; + (*ret) = vi; + return true; + } - if (index0 >= 0 && (index0) < numTexCoords) - { - texcoords.push_back(in_texcoords[index0]); - } - if (index1 >= 0 && (index1) < numTexCoords) - { - texcoords.push_back(in_texcoords[index1]); - } + // i/j/k + (*token)++; // skip '/' + if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) + { + return false; } + (*token) += strcspn((*token), "/ \t\r"); - unsigned int idx = positions.size() / 3 - 1; - vertexCache[i] = idx; + (*ret) = vi; - return idx; + return true; } -static bool -exportFaceGroupToShape( - shape_t& shape, - const std::vector<float>& in_positions, - const std::vector<float>& in_normals, - const std::vector<float>& in_texcoords, - const std::vector<MyIndices>& faceGroup, - const material_t material, - const std::string name, - std::vector<vertex_index>& allIndices) +static bool exportFaceGroupToShape(shape_t* shape, const std::vector<face_t>& face_group, + const material_t material, const std::string& name, + const std::vector<float>& v) { - if (faceGroup.empty()) + if (face_group.empty()) { return false; } + shape->name = name; // Flattened version of vertex data - std::vector<float> positions; - std::vector<float> normals; - std::vector<float> texcoords; - std::map<vertex_index, unsigned int> vertexCache; - std::vector<unsigned int> indices; // Flatten vertices and indices - for (size_t i = 0; i < faceGroup.size(); i++) + for (size_t i = 0; i < face_group.size(); i++) { - const MyIndices& face = faceGroup[i]; + const face_t& face = face_group[i]; + size_t npolys = face.size(); + + if (npolys < 3) + { + // Face must have 3+ vertices. + continue; + } + vertex_index_t i0 = face[0]; + vertex_index_t i1(-1); + vertex_index_t i2 = face[1]; - vertex_index i0 = allIndices[face.m_offset]; - vertex_index i1; - i1.vn_idx = -1; - i1.vt_idx = -1; - i1.v_idx = -1; - vertex_index i2 = allIndices[face.m_offset + 1]; + face_t remainingFace = face; // copy + size_t guess_vert = 0; + vertex_index_t ind[3]; - size_t npolys = face.m_numIndices; //.size(); + // How many iterations can we do without decreasing the remaining + // vertices. + size_t remainingIterations = face.size(); + size_t previousRemainingVertices = remainingFace.size(); + while (remainingFace.size() > 3 && remainingIterations > 0) { - // Polygon -> triangle fan conversion - for (size_t k = 2; k < npolys; k++) + npolys = remainingFace.size(); + if (guess_vert >= npolys) { - i1 = i2; - i2 = allIndices[face.m_offset + k]; - - unsigned int v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0); - unsigned int v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1); - unsigned int v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2); + guess_vert -= npolys; + } - indices.push_back(v0); - indices.push_back(v1); - indices.push_back(v2); + if (previousRemainingVertices != npolys) + { + // The number of remaining vertices decreased. Reset counters. + previousRemainingVertices = npolys; + remainingIterations = npolys; + } + else + { + // We didn't consume a vertex on previous iteration, reduce the + // available iterations. + remainingIterations--; } - } - } - // - // Construct shape. - // - shape.name = name; - shape.mesh.positions.swap(positions); - shape.mesh.normals.swap(normals); - shape.mesh.texcoords.swap(texcoords); - shape.mesh.indices.swap(indices); + for (size_t k = 0; k < 3; k++) + { + ind[k] = remainingFace[(guess_vert + k) % npolys]; + size_t vi = size_t(ind[k].v_idx); + } + // this triangle is an ear + { + index_t idx0, idx1, idx2; + idx0.vertex_index = ind[0].v_idx; + idx0.normal_index = ind[0].vn_idx; + idx0.texcoord_index = ind[0].vt_idx; + idx1.vertex_index = ind[1].v_idx; + idx1.normal_index = ind[1].vn_idx; + idx1.texcoord_index = ind[1].vt_idx; + idx2.vertex_index = ind[2].v_idx; + idx2.normal_index = ind[2].vn_idx; + idx2.texcoord_index = ind[2].vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + } - shape.material = material; + // remove v1 from the list + size_t removed_vert_index = (guess_vert + 1) % npolys; + while (removed_vert_index + 1 < npolys) + { + remainingFace[removed_vert_index] = + remainingFace[removed_vert_index + 1]; + removed_vert_index += 1; + } + remainingFace.pop_back(); + } + if (remainingFace.size() == 3) + { + i0 = remainingFace[0]; + i1 = remainingFace[1]; + i2 = remainingFace[2]; + { + index_t idx0, idx1, idx2; + idx0.vertex_index = i0.v_idx; + idx0.normal_index = i0.vn_idx; + idx0.texcoord_index = i0.vt_idx; + idx1.vertex_index = i1.v_idx; + idx1.normal_index = i1.vn_idx; + idx1.texcoord_index = i1.vt_idx; + idx2.vertex_index = i2.v_idx; + idx2.normal_index = i2.vn_idx; + idx2.texcoord_index = i2.vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + } + } + } + shape->material = material; return true; } @@ -329,7 +332,6 @@ void InitMaterial(material_t& material) material.ambient_texname = ""; material.diffuse_texname = ""; material.specular_texname = ""; - material.normal_texname = ""; for (int i = 0; i < 3; i++) { material.ambient[i] = 0.f; @@ -369,8 +371,8 @@ std::string LoadMtl( return err.str(); } #else - int fileHandle = fileIO->fileOpen(filepath.c_str(),"r"); - if (fileHandle<0) + int fileHandle = fileIO->fileOpen(filepath.c_str(), "r"); + if (fileHandle < 0) { err << "Cannot open file [" << filepath << "]" << std::endl; return err.str(); @@ -396,7 +398,7 @@ std::string LoadMtl( line = fileIO->readLine(fileHandle, tmpBuf, 1024); if (line) { - linebuf=line; + linebuf = line; } #endif @@ -420,7 +422,7 @@ std::string LoadMtl( // Skip leading space. const char* token = linebuf.c_str(); token += strspn(token, " \t"); - + assert(token); if (token[0] == '\0') continue; // empty line @@ -574,12 +576,13 @@ std::string LoadMtl( } } #ifndef USE_STREAM - while (line); + while (line) + ; #endif // flush last material. material_map.insert(std::pair<std::string, material_t>(material.name, material)); - if (fileHandle>=0) + if (fileHandle >= 0) { fileIO->fileClose(fileHandle); } @@ -588,11 +591,16 @@ std::string LoadMtl( std::string LoadObj( + attrib_t& attrib, std::vector<shape_t>& shapes, const char* filename, const char* mtl_basepath, CommonFileIOInterface* fileIO) { + attrib.vertices.clear(); + attrib.normals.clear(); + attrib.texcoords.clear(); + shapes.clear(); std::string tmp = filename; if (!mtl_basepath) { @@ -604,13 +612,6 @@ LoadObj( mtl_basepath = tmp.c_str(); //fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename); } - - shapes.resize(0); - std::vector<vertex_index> allIndices; - allIndices.reserve(1024 * 1024); - - MyIndices face; - std::stringstream err; #ifdef USE_STREAM std::ifstream ifs(filename); @@ -620,8 +621,8 @@ LoadObj( return err.str(); } #else - int fileHandle = fileIO->fileOpen(filename,"r"); - if (fileHandle<0) + int fileHandle = fileIO->fileOpen(filename, "r"); + if (fileHandle < 0) { err << "Cannot open file [" << filename << "]" << std::endl; return err.str(); @@ -629,16 +630,15 @@ LoadObj( #endif std::vector<float> v; - v.reserve(1024 * 1024); std::vector<float> vn; - vn.reserve(1024 * 1024); std::vector<float> vt; - vt.reserve(1024 * 1024); - //std::vector<std::vector<vertex_index> > faceGroup; - std::vector<MyIndices> faceGroup; - faceGroup.reserve(1024 * 1024); std::string name; + int greatest_v_idx = -1; + int greatest_vn_idx = -1; + int greatest_vt_idx = -1; + + std::vector<face_t> faceGroup; // material std::map<std::string, material_t> material_map; material_t material; @@ -664,10 +664,9 @@ LoadObj( line = fileIO->readLine(fileHandle, tmpBuf, 1024); if (line) { - linebuf=line; + linebuf = line; } #endif - // Trim newline '\r\n' or '\r' if (linebuf.size() > 0) { @@ -734,23 +733,34 @@ LoadObj( token += 2; token += strspn(token, " \t"); - face.m_offset = allIndices.size(); - face.m_numIndices = 0; + face_t face; + + face.reserve(3); while (!isNewLine(token[0])) { - vertex_index vi = parseTriple(token, v.size() / 3, vn.size() / 3, vt.size() / 2); - allIndices.push_back(vi); - face.m_numIndices++; - int n = strspn(token, " \t\r"); + vertex_index_t vi; + if (!parseTriple(&token, static_cast<int>(v.size() / 3), + static_cast<int>(vn.size() / 3), + static_cast<int>(vt.size() / 2), &vi)) + { + err << "Failed parse `f' line(e.g. zero value for face index."; + return err.str(); + } + + greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx; + greatest_vn_idx = + greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx; + greatest_vt_idx = + greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx; + + face.push_back(vi); + size_t n = strspn(token, " \t\r"); token += n; } - faceGroup.push_back(face); - continue; } - // use mtl if ((0 == strncmp(token, "usemtl", 6)) && isSpace((token[6]))) { @@ -777,10 +787,10 @@ LoadObj( token += 7; sscanf(token, "%s", namebuf); - std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO); + std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath, fileIO); if (!err_mtl.empty()) { - //faceGroup.resize(0); // for safety + //face_group.resize(0); // for safety //return err_mtl; } continue; @@ -791,7 +801,7 @@ LoadObj( { // flush previous face group. shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); @@ -827,7 +837,7 @@ LoadObj( { // flush previous face group. shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); @@ -847,18 +857,23 @@ LoadObj( // Ignore unknown command. } #ifndef USE_STREAM - while (line); + while (line) + ; #endif shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); } faceGroup.resize(0); // for safety - if (fileHandle>=0) + attrib.vertices.swap(v); + attrib.normals.swap(vn); + attrib.texcoords.swap(vt); + + if (fileHandle >= 0) { fileIO->fileClose(fileHandle); } diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h index 0319b7c0d..1fb15b34a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h @@ -14,6 +14,17 @@ struct CommonFileIOInterface; namespace tinyobj { +struct vertex_index_t +{ + int v_idx, vt_idx, vn_idx; + vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {} + explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {} + vertex_index_t(int vidx, int vtidx, int vnidx) + : v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {} +}; + +typedef std::vector<vertex_index_t> face_t; + typedef struct { std::string name; @@ -24,21 +35,27 @@ typedef struct float transmittance[3]; float emission[3]; float shininess; - float transparency; + float transparency; // 1 == opaque; 0 == fully transparent - std::string ambient_texname; - std::string diffuse_texname; - std::string specular_texname; + std::string ambient_texname; // map_Ka + std::string diffuse_texname; // map_Kd + std::string specular_texname; // map_Ks std::string normal_texname; std::map<std::string, std::string> unknown_parameter; } material_t; +// Index struct to support different indices for vtx/normal/texcoord. +// -1 means not used. typedef struct { - std::vector<float> positions; - std::vector<float> normals; - std::vector<float> texcoords; - std::vector<unsigned int> indices; + int vertex_index; + int normal_index; + int texcoord_index; +} index_t; + +typedef struct +{ + std::vector<index_t> indices; } mesh_t; typedef struct @@ -48,6 +65,14 @@ typedef struct mesh_t mesh; } shape_t; +// Vertex attributes +struct attrib_t +{ + std::vector<float> vertices; // 'v'(xyz) + std::vector<float> normals; // 'vn' + std::vector<float> texcoords; // 'vt'(uv) + attrib_t() {} +}; /// Loads .obj from a file. /// 'shapes' will be filled with parsed shape data /// The function returns error string. @@ -55,12 +80,14 @@ typedef struct /// 'mtl_basepath' is optional, and used for base path for .mtl file. #ifdef USE_STREAM std::string LoadObj( + attrib_t& attrib, std::vector<shape_t>& shapes, // [output] const char* filename, const char* mtl_basepath = NULL); #else std::string LoadObj( + attrib_t& attrib, std::vector<shape_t>& shapes, const char* filename, const char* mtl_basepath, diff --git a/src/.DS_Store b/src/.DS_Store Binary files differnew file mode 100644 index 000000000..ee616d543 --- /dev/null +++ b/src/.DS_Store diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index e5097ccbb..57e1bb494 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState() == ACTIVE_TAG || @@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 7fe961921..73607c61f 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define BT_DISCRETE_DYNAMICS_WORLD_H #include "btDynamicsWorld.h" - class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; @@ -26,6 +25,7 @@ class btTypedConstraint; class btActionInterface; class btPersistentManifold; class btIDebugDraw; + struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" @@ -76,7 +76,7 @@ protected: virtual void calculateSimulationIslands(); - virtual void solveConstraints(btContactSolverInfo & solverInfo); + virtual void updateActivationState(btScalar timeStep); @@ -95,7 +95,7 @@ protected: void serializeRigidBodies(btSerializer * serializer); void serializeDynamicsWorldInfo(btSerializer * serializer); - + public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -107,6 +107,8 @@ public: ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + virtual void solveConstraints(btContactSolverInfo & solverInfo); + virtual void synchronizeMotionStates(); ///this can be useful to synchronize a single rigid body -> graphics object @@ -227,6 +229,16 @@ public: { return m_latencyMotionStateInterpolation; } + + btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() + { + return m_nonStaticRigidBodies; + } + + const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const + { + return m_nonStaticRigidBodies; + } }; #endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index eadd8c12e..3c55234a8 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -34,7 +34,8 @@ enum btDynamicsWorldType BT_CONTINUOUS_DYNAMICS_WORLD = 3, BT_SOFT_RIGID_DYNAMICS_WORLD = 4, BT_GPU_DYNAMICS_WORLD = 5, - BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6 + BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6, + BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 3e210d752..4d634b699 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -100,6 +100,8 @@ btMultiBody::btMultiBody(int n_links, m_baseName(0), m_basePos(0, 0, 0), m_baseQuat(0, 0, 0, 1), + m_basePos_interpolate(0, 0, 0), + m_baseQuat_interpolate(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -449,6 +451,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const return m_links[i].m_cachedRotParentToThis; } +const btVector3 &btMultiBody::getInterpolateRVector(int i) const +{ + return m_links[i].m_cachedRVector_interpolate; +} + +const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis_interpolate; +} + btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { btAssert(i >= -1); @@ -1581,6 +1593,158 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar //printf("]\n"); ///////////////// } +void btMultiBody::predictPositionsMultiDof(btScalar dt) +{ + int num_links = getNumLinks(); + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + + /////////////////////////////// + //local functor for quaternion integration (to avoid error prone redundancy) + struct + { + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if (!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; + } + + if (fAngle < btScalar(0.001)) + { + // use Taylor's expansions of sync function + axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); + } + + if (!baseBody) + quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; + else + quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + btScalar *pBaseQuat; + + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + // Finally we can update m_jointPos for each of the m_links + for (int i = 0; i < num_links; ++i) + { + btScalar *pJointPos; + pJointPos = &m_links[i].m_jointPos_interpolate[0]; + + btScalar *pJointVel = getJointVelMultiDof(i); + + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + pJointPos[0] = m_links[i].m_jointPos[0]; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + + for (int i = 0; i < 4; ++i) + { + pJointPos[i] = m_links[i].m_jointPos[i]; + } + + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + for (int i = 0; i < 3; ++i) + { + pJointPos[i] = m_links[i].m_jointPos[i]; + } + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + break; + } + default: + { + } + } + + m_links[i].updateInterpolationCacheMultiDof(); + } +} void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { @@ -1589,9 +1753,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //btVector3 v = getBaseVel(); //m_basePos += dt * v; // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + pBasePos[0] += dt * pBaseVel[0]; pBasePos[1] += dt * pBaseVel[1]; pBasePos[2] += dt * pBaseVel[2]; @@ -1645,7 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseQuat = pq ? pq : m_baseQuat; btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // btQuaternion baseQuat; @@ -1670,7 +1834,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); switch (m_links[i].m_jointType) @@ -1678,12 +1844,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { + //reset to current pos btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { + //reset to current pos btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); btQuaternion jointOri; @@ -2006,6 +2174,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu } } +void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) +{ + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getBaseCollider()->setInterpolationWorldTransform(tr); + } + + for (int k = 0; k < getNumLinks(); k++) + { + const int parent = getParent(k); + world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); + } + + for (int m = 0; m < getNumLinks(); m++) + { + btMultiBodyLinkCollider *col = getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + col->setInterpolationWorldTransform(tr); + } + } +} + int btMultiBody::calculateSerializeBufferSize() const { int sz = sizeof(btMultiBodyData); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c0b0d003b..91b5c3edb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -193,12 +193,24 @@ public: const btQuaternion &getWorldToBaseRot() const { return m_baseQuat; - } // rotates world vectors into base frame + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame void setBasePos(const btVector3 &pos) { m_basePos = pos; + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -224,6 +236,7 @@ public: void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } void setBaseOmega(const btVector3 &omega) { @@ -273,6 +286,8 @@ public: const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. // // transform vectors in local frame of link i to world frame (or vice versa) @@ -421,6 +436,9 @@ public: // timestep the positions (given current velocities). void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); // // contacts @@ -581,6 +599,7 @@ public: void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); virtual int calculateSerializeBufferSize() const; @@ -664,7 +683,9 @@ private: const char *m_baseName; //memory needs to be manager by user! btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; btScalar m_baseMass; // mass of the base btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1131e5378..1be76ad86 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) m_multiBodies.remove(body); } +void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + predictMultiBodyTransforms(timeStep); + +} void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); @@ -421,292 +427,19 @@ void btMultiBodyDynamicsWorld::forwardKinematics() } void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - forwardKinematics(); - - BT_PROFILE("solveConstraints"); - - clearMultiBodyConstraintForces(); - - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); - - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - - m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - { - BT_PROFILE("btMultiBody addForce"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - } //if (!isSleeping) - } - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool doNotUpdatePos = false; - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray<btScalar> scratch_r2; - scratch_r2.resize(2 * numPosVars + 8 * numDofs); - //convenience - btScalar* pMem = &scratch_r2[0]; - btScalar* scratch_q0 = pMem; - pMem += numPosVars; - btScalar* scratch_qx = pMem; - pMem += numPosVars; - btScalar* scratch_qd0 = pMem; - pMem += numDofs; - btScalar* scratch_qd1 = pMem; - pMem += numDofs; - btScalar* scratch_qd2 = pMem; - pMem += numDofs; - btScalar* scratch_qd3 = pMem; - pMem += numDofs; - btScalar* scratch_qdd0 = pMem; - pMem += numDofs; - btScalar* scratch_qdd1 = pMem; - pMem += numDofs; - btScalar* scratch_qdd2 = pMem; - pMem += numDofs; - btScalar* scratch_qdd3 = pMem; - pMem += numDofs; - btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for (int link = 0; link < bod->getNumLinks(); ++link) - { - for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for (int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody* bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) - { - for (int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } - - } pEulerIntegrate; - // - struct - { - void operator()(btMultiBody* pBody, const btScalar* pData) - { - btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); - - for (int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - } - } pCopyToVelocityVector; - // - struct - { - void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) - { - for (int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; -#define output &m_scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray<btScalar> delta_q; - delta_q.resize(numDofs); - btAlignedObjectArray<btScalar> delta_qd; - delta_qd.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if (!doNotUpdatePos) - { - btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - for (int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for (int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } + solveExternalForces(solverInfo); + buildIslands(); + solveInternalConstraints(solverInfo); +} -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - } //if (!isSleeping) - } - } +void btMultiBodyDynamicsWorld::buildIslands() +{ + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); +} +void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) +{ /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); - m_solverMultiBodyIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); @@ -760,11 +493,306 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } +void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) +{ + forwardKinematics(); + + BT_PROFILE("solveConstraints"); + + clearMultiBodyConstraintForces(); + + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + { + BT_PROFILE("btMultiBody addForce"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool doNotUpdatePos = false; + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + const btScalar linearDamp = bod->getLinearDamping(); +// const btScalar angularDamp = bod->getAngularDamping(); + bod->setLinearDamping(0); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + bod->setLinearDamping(linearDamp); +// bod->setAngularDamping(angularDamp); + } + else + { + // + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray<btScalar> scratch_r2; + scratch_r2.resize(2 * numPosVars + 8 * numDofs); + //convenience + btScalar* pMem = &scratch_r2[0]; + btScalar* scratch_q0 = pMem; + pMem += numPosVars; + btScalar* scratch_qx = pMem; + pMem += numPosVars; + btScalar* scratch_qd0 = pMem; + pMem += numDofs; + btScalar* scratch_qd1 = pMem; + pMem += numDofs; + btScalar* scratch_qd2 = pMem; + pMem += numDofs; + btScalar* scratch_qd3 = pMem; + pMem += numDofs; + btScalar* scratch_qdd0 = pMem; + pMem += numDofs; + btScalar* scratch_qdd1 = pMem; + pMem += numDofs; + btScalar* scratch_qdd2 = pMem; + pMem += numDofs; + btScalar* scratch_qdd3 = pMem; + pMem += numDofs; + btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); + + ///// + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // + for (int link = 0; link < bod->getNumLinks(); ++link) + { + for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + } + // + for (int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// + struct + { + btMultiBody* bod; + btScalar *scratch_qx, *scratch_q0; + + void operator()() + { + for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) + { + for (int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } + + } pEulerIntegrate; + // + struct + { + void operator()(btMultiBody* pBody, const btScalar* pData) + { + btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) + { + for (int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; +#define output &m_scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd0, 0, numDofs); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd1, 0, numDofs); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd2, 0, numDofs); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd3, 0, numDofs); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray<btScalar> delta_q; + delta_q.resize(numDofs); + btAlignedObjectArray<btScalar> delta_qd; + delta_qd.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; + } + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + if (!doNotUpdatePos) + { + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + for (int i = 0; i < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); + } + + //ugly hack which resets the cached data to t0 (needed for constraint solver) + { + for (int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + bod->clearForcesAndTorques(); +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + } //if (!isSleeping) + } + } +} + + void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); + integrateMultiBodyTransforms(timeStep); +} - { +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) +{ BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -787,31 +815,61 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) int nLinks = bod->getNumLinks(); ///base + num m_links + if (!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); + else + { + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - { - if (!bod->isPosUpdated()) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->setPosUpdated(false); - } - } m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { bod->clearVelocities(); } } - } +} + +void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep) +{ + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + + for (int b = 0; b < m_multiBodies.size(); b++) + { + btMultiBody* bod = m_multiBodies[b]; + bool isSleeping = false; + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + bod->predictPositionsMultiDof(timeStep); + m_scratch_world_to_local.resize(nLinks + 1); + m_scratch_local_origin.resize(nLinks + 1); + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + } + else + { + bod->clearVelocities(); + } + } } void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index e36c2f7aa..653ec36ca 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -47,7 +47,7 @@ protected: virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void serializeMultiBodies(btSerializer* serializer); @@ -55,7 +55,8 @@ public: btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); virtual ~btMultiBodyDynamicsWorld(); - + + virtual void solveConstraints(btContactSolverInfo& solverInfo); virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); virtual void removeMultiBody(btMultiBody* body); @@ -95,7 +96,10 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - + void integrateMultiBodyTransforms(btScalar timeStep); + void predictMultiBodyTransforms(btScalar timeStep); + + virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); @@ -110,6 +114,10 @@ public: virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); virtual void setConstraintSolver(btConstraintSolver* solver); virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const; + + virtual void solveExternalForces(btContactSolverInfo& solverInfo); + virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); + void buildIslands(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 92d41dfac..1cd6b8c07 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -111,6 +111,10 @@ struct btMultibodyLink btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame @@ -119,6 +123,7 @@ struct btMultibodyLink btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; + btScalar m_jointPos_interpolate[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. //It gets set to zero after each internal stepSimulation call @@ -188,42 +193,43 @@ struct btMultibodyLink // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector =m_cachedRVector; switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); break; } case eFixed: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } @@ -234,6 +240,57 @@ struct btMultibodyLink } } } + + void updateInterpolationCacheMultiDof() + { + btScalar *pJointPos = &m_jointPos_interpolate[0]; + + btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; + btVector3& cachedVector = m_cachedRVector_interpolate; + switch (m_jointType) + { + case eRevolute: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + case ePlanar: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); + + break; + } + case eFixed: + { + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } }; #endif //BT_MULTIBODY_LINK_H diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index d43df1c67..40155786e 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -17,6 +17,11 @@ SET(BulletSoftBody_SRCS btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp + btDeformableBackwardEulerObjective.cpp + btDeformableBodySolver.cpp + btDeformableContactProjection.cpp + btDeformableRigidDynamicsWorld.cpp + ) SET(BulletSoftBody_HDRS @@ -33,6 +38,18 @@ SET(BulletSoftBody_HDRS btSoftBodySolvers.h btDefaultSoftBodySolver.h + + btCGProjection.h + btConjugateGradient.h + btDeformableGravityForce.h + btDeformableMassSpringForce.h + btDeformableLagrangianForce.h + btPreconditioner.h + + btDeformableBackwardEulerObjective.h + btDeformableBodySolver.h + btDeformableContactProjection.h + btDeformableRigidDynamicsWorld.h btSoftBodySolverVertexBuffer.h ) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h new file mode 100644 index 000000000..f38fa545c --- /dev/null +++ b/src/BulletSoftBody/btCGProjection.h @@ -0,0 +1,148 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_CG_PROJECTION_H +#define BT_CG_PROJECTION_H + +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" + +class btDeformableRigidDynamicsWorld; + +struct DeformableContactConstraint +{ + btAlignedObjectArray<const btSoftBody::RContact*> m_contact; + btAlignedObjectArray<btVector3> m_direction; + btAlignedObjectArray<btScalar> m_value; + // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve + btAlignedObjectArray<btScalar> m_accumulated_normal_impulse; + + DeformableContactConstraint(const btSoftBody::RContact& rcontact) + { + append(rcontact); + } + + DeformableContactConstraint(const btVector3 dir) + { + m_contact.push_back(NULL); + m_direction.push_back(dir); + m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); + } + + DeformableContactConstraint() + { + m_contact.push_back(NULL); + m_direction.push_back(btVector3(0,0,0)); + m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_direction.push_back(rcontact.m_cti.m_normal); + m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); + } + + ~DeformableContactConstraint() + { + } +}; + +struct DeformableFrictionConstraint +{ + btAlignedObjectArray<bool> m_static; // whether the friction is static + btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels + btAlignedObjectArray<btScalar> m_dv; // the dv magnitude of the node + btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node + + + btAlignedObjectArray<bool> m_static_prev; + btAlignedObjectArray<btScalar> m_impulse_prev; + btAlignedObjectArray<btScalar> m_dv_prev; + btAlignedObjectArray<btVector3> m_direction_prev; + + btAlignedObjectArray<bool> m_released; // whether the contact is released + + + // the total impulse the node applied to the rb in the tangential direction in the cg solve + btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse; + + DeformableFrictionConstraint() + { + append(); + } + + void append() + { + m_static.push_back(false); + m_static_prev.push_back(false); + + m_direction_prev.push_back(btVector3(0,0,0)); + m_direction.push_back(btVector3(0,0,0)); + + m_impulse.push_back(0); + m_impulse_prev.push_back(0); + + m_dv.push_back(0); + m_dv_prev.push_back(0); + + m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); + m_released.push_back(false); + } +}; + +class btCGProjection +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; + btAlignedObjectArray<btSoftBody *>& m_softBodies; + btDeformableRigidDynamicsWorld* m_world; +// const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + const btScalar& m_dt; + + btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) + : m_softBodies(softBodies) + , m_dt(dt) + { + } + + virtual ~btCGProjection() + { + } + + // apply the constraints + virtual void project(TVStack& x) = 0; + + virtual void setConstraints() = 0; + + // update the constraints + virtual void update() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual void setWorld(btDeformableRigidDynamicsWorld* world) + { + m_world = world; + } +}; + + +#endif /* btCGProjection_h */ diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h new file mode 100644 index 000000000..22a9f3ada --- /dev/null +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -0,0 +1,146 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_CONJUGATE_GRADIENT_H +#define BT_CONJUGATE_GRADIENT_H +#include <iostream> +#include <cmath> +#include <LinearMath/btAlignedObjectArray.h> +#include <LinearMath/btVector3.h> +#include "LinearMath/btQuickprof.h" +template <class MatrixX> +class btConjugateGradient +{ + typedef btAlignedObjectArray<btVector3> TVStack; + TVStack r,p,z,temp; + int max_iterations; +public: + btConjugateGradient(const int max_it_in) + : max_iterations(max_it_in) + { + } + + virtual ~btConjugateGradient(){} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance) + { + BT_PROFILE("CGSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = sub(b, temp); + A.project(r); + A.enforceConstraint(x); + + btScalar r_norm = std::sqrt(squaredNorm(r)); + if (r_norm < tolerance) { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_norm << std::endl; + return 0; + } + + // z = M^(-1) * r + A.precondition(r, z); + p = z; + // temp = A*p + A.multiply(p, temp); + + btScalar r_dot_z = dot(z,r), r_dot_z_new; + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z / dot(p, temp), beta; + + for (int k = 1; k < max_iterations; k++) { + // x += alpha * p; + // r -= alpha * temp; + multAndAddTo(alpha, p, x); + multAndAddTo(-alpha, temp, r); + // zero out the dofs of r + A.project(r); + A.enforceConstraint(x); + r_norm = std::sqrt(squaredNorm(r)); + + if (r_norm < tolerance) { + std::cout << "ConjugateGradient iterations " << k << std::endl; + return k; + } + + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z_new = dot(r,z); + beta = r_dot_z_new/ r_dot_z; + r_dot_z = r_dot_z_new; + // p = z + beta * p; + p = multAndAdd(beta, p, z); + // temp = A * p; + A.multiply(p, temp); + // alpha = r^T * z / (p^T * A * p) + alpha = r_dot_z / dot(p, temp); + } + std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; + return max_iterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } + + TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()) + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + c[i] = a[i] - b[i]; + return c; + } + + btScalar squaredNorm(const TVStack& a) + { + return dot(a,a); + } + + btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { +// result += s*a + btAssert(a.size() == result.size()) + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } +}; +#endif /* btConjugateGradient_h */ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp new file mode 100644 index 000000000..746048562 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -0,0 +1,147 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "btDeformableBackwardEulerObjective.h" +#include "LinearMath/btQuickprof.h" + +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) +: m_softBodies(softBodies) +, projection(m_softBodies, m_dt) +, m_backupVelocity(backup_v) +{ + m_preconditioner = new DefaultPreconditioner(); +} + +void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) +{ + BT_PROFILE("reinitialize"); + setDt(dt); + if(nodeUpdated) + { + updateId(); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + projection.reinitialize(nodeUpdated); + m_preconditioner->reinitialize(nodeUpdated); +} + +void btDeformableBackwardEulerObjective::setDt(btScalar dt) +{ + m_dt = dt; +} + +void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const +{ + BT_PROFILE("multiply"); + // add in the mass term + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledForceDifferential(-m_dt, x, b); + } +} + +void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) +{ + // only the velocity of the constrained nodes needs to be updated during CG solve + for (int i = 0; i < projection.m_constraints.size(); ++i) + { + int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); + m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; + } +} + +void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) +{ + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } +} + +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const +{ + BT_PROFILE("computeResidual"); + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledImplicitForce(dt, residual); + } +} + +btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const +{ + btScalar norm_squared = 0; + for (int i = 0; i < residual.size(); ++i) + { + norm_squared += residual[i].length2(); + } + return std::sqrt(norm_squared+SIMD_EPSILON); +} + +void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) +{ + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + applyForce(force, true); +} + +void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) +{ + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } +} + +//set constraints as projections +void btDeformableBackwardEulerObjective::setConstraints() +{ + // build islands for multibody solve + m_world->btMultiBodyDynamicsWorld::buildIslands(); + projection.setConstraints(); +} diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h new file mode 100644 index 000000000..f87f7f509 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -0,0 +1,126 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_BACKWARD_EULER_OBJECTIVE_H +#define BT_BACKWARD_EULER_OBJECTIVE_H +#include "btConjugateGradient.h" +#include "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" +#include "btDeformableGravityForce.h" +#include "btDeformableContactProjection.h" +#include "btPreconditioner.h" +#include "btDeformableRigidDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +class btDeformableRigidDynamicsWorld; +class btDeformableBackwardEulerObjective +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_dt; + btDeformableRigidDynamicsWorld* m_world; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btSoftBody *>& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection projection; + const TVStack& m_backupVelocity; + btAlignedObjectArray<btSoftBody::Node* > m_nodes; + btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective() {} + + void initialize(){} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual) const; + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + + // compute one step of the solve (there is only one solve if the system is linear) + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + + // perform A*x = b + void multiply(const TVStack& x, TVStack& b) const; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); + + void setDt(btScalar dt); + + // enforce constraints in CG solve + void enforceConstraint(TVStack& x) + { + BT_PROFILE("enforceConstraint"); + projection.enforceConstraint(x); + updateVelocity(x); + } + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints(); + + // update the projections and project the residual + void project(TVStack& r) + { + BT_PROFILE("project"); + projection.update(); + projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x,b); + } + + virtual void setWorld(btDeformableRigidDynamicsWorld* world) + { + m_world = world; + projection.setWorld(world); + } + + virtual void updateId() + { + size_t id = 0; + m_nodes.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].index = id; + m_nodes.push_back(&psb->m_nodes[j]); + ++id; + } + } + } + + const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const + { + return &m_nodes; + } +}; + +#endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp new file mode 100644 index 000000000..eb8e62237 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -0,0 +1,204 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include <stdio.h> +#include <limits> +#include "btDeformableBodySolver.h" +#include "LinearMath/btQuickprof.h" + +btDeformableBodySolver::btDeformableBodySolver() +: m_numNodes(0) +, m_cg(10) +{ + m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); +} + +btDeformableBodySolver::~btDeformableBodySolver() +{ + delete m_objective; +} + +void btDeformableBodySolver::solveConstraints(float solverdt) +{ + BT_PROFILE("solveConstraints"); + // add constraints to the solver + setConstraints(); + + // save v_{n+1}^* velocity after explicit forces + backupVelocity(); + + m_objective->computeResidual(solverdt, m_residual); + + computeStep(m_dv, m_residual); + updateVelocity(); +} + +void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) +{ + btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual); + m_cg.solve(*m_objective, dv, residual, tolerance); +} + +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt) +{ + m_softBodySet.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); + + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0,0,0)); + m_residual.resize(m_numNodes, btVector3(0,0,0)); + m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); + } + + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_residual[i].setZero(); + } + + m_objective->reinitialize(nodeUpdated, dt); +} + +void btDeformableBodySolver::setConstraints() +{ + BT_PROFILE("setConstraint"); + m_objective->setConstraints(); +} + +void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) +{ + m_objective->setWorld(world); +} + +void btDeformableBodySolver::updateVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; + ++counter; + } + } +} + +void btDeformableBodySolver::backupVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } +} + +void btDeformableBodySolver::revertVelocity() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } +} + +bool btDeformableBodySolver::updateNodes() +{ + int numNodes = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + numNodes += m_softBodySet[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + return true; + } + return false; +} + + +void btDeformableBodySolver::predictMotion(float solverdt) +{ + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody *psb = m_softBodySet[i]; + + if (psb->isActive()) + { + // apply explicit forces to velocity + m_objective->applyExplicitForce(m_residual); + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); + } + } +} + +void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) +{ + int i, ni; + + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Integrate */ + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_q = n.m_x; + n.m_x += n.m_v * dt; + } + /* Bounds */ + psb->updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg); + psb->m_ndbvt.update(n.m_leaf, + vol, + n.m_v * psb->m_sst.velmrg, + psb->m_sst.updmrg); + } + + /* Clear contacts */ + psb->m_rcontacts.resize(0); + psb->m_scontacts.resize(0); + /* Optimize dbvt's */ + psb->m_ndbvt.optimizeIncremental(1); +} + +void btDeformableBodySolver::updateSoftBodies() +{ + for (int i = 0; i < m_softBodySet.size(); i++) + { + btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; + if (psb->isActive()) + { + psb->updateNormals(); // normal is updated here + } + } +} diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h new file mode 100644 index 000000000..94102afa9 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -0,0 +1,94 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_BODY_SOLVERS_H +#define BT_DEFORMABLE_BODY_SOLVERS_H + + +#include "btSoftBodySolvers.h" +#include "btDeformableBackwardEulerObjective.h" +#include "btDeformableRigidDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" + +struct btCollisionObjectWrapper; +class btDeformableBackwardEulerObjective; +class btDeformableRigidDynamicsWorld; + +class btDeformableBodySolver : public btSoftBodySolver +{ +// using TVStack = btAlignedObjectArray<btVector3>; + typedef btAlignedObjectArray<btVector3> TVStack; +protected: + int m_numNodes; + TVStack m_dv; + TVStack m_residual; + btAlignedObjectArray<btSoftBody *> m_softBodySet; + + btAlignedObjectArray<btVector3> m_backupVelocity; + btScalar m_dt; + btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; + + +public: + btDeformableBackwardEulerObjective* m_objective; + + btDeformableBodySolver(); + + virtual ~btDeformableBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + virtual void updateSoftBodies(); + + virtual void copyBackToSoftBodies(bool bMove = true) {} + + void extracted(float solverdt); + + virtual void solveConstraints(float solverdt); + + void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt); + + void setConstraints(); + + void predictDeformableMotion(btSoftBody* psb, btScalar dt); + + void backupVelocity(); + void revertVelocity(); + void updateVelocity(); + + bool updateNodes(); + + void computeStep(TVStack& dv, const TVStack& residual); + + virtual void predictMotion(float solverdt); + + virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} + + virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { + softBody->defaultCollisionHandler(otherSoftBody); + } + virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){} + virtual bool checkInitialized(){return true;} + virtual void setWorld(btDeformableRigidDynamicsWorld* world); +}; + +#endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp new file mode 100644 index 000000000..84d35c062 --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -0,0 +1,478 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "btDeformableContactProjection.h" +#include "btDeformableRigidDynamicsWorld.h" +#include <algorithm> +#include <cmath> +static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} + +static btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} + +void btDeformableContactProjection::update() +{ + ///solve rigid body constraints + m_world->getSolverInfo().m_numIterations = 1; + m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); + + // loop through constraints to set constrained values + for (int index = 0; index < m_constraints.size(); ++index) + { + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) + { + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) + { + if (constraint.m_contact[j] == NULL) + { + // nothing needs to be done for dirichelet constraints + continue; + } + const btSoftBody::RContact* c = constraint.m_contact[j]; + const btSoftBody::sCti& cti = c->m_cti; + + if (cti.m_colObj->hasContactResponse()) + { + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + const btScalar* deltaV_normal; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_n[k]; + } + va = cti.m_normal * vel * m_dt; + + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t1[k]; + } + va += c->t1 * vel * m_dt; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t2[k]; + } + va += c->t2 * vel * m_dt; + } + } + + const btVector3 vb = c->m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + btVector3 impulse = c->m_c0 * vr; + const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn); + const btVector3 impulse_tangent = impulse - impulse_normal; + + // start friction handling + // copy old data + friction.m_impulse_prev[j] = friction.m_impulse[j]; + friction.m_dv_prev[j] = friction.m_dv[j]; + friction.m_static_prev[j] = friction.m_static[j]; + + // get the current tangent direction + btScalar local_tangent_norm = impulse_tangent.norm(); + btVector3 local_tangent_dir = btVector3(0,0,0); + if (local_tangent_norm > SIMD_EPSILON) + local_tangent_dir = impulse_tangent.normalized(); + + // accumulated impulse on the rb in this and all prev cg iterations + constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal); + const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j]; + + // the total tangential impulse required to stop sliding + btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent; + btScalar tangent_norm = tangent.norm(); + + if (accumulated_normal < 0) + { + friction.m_direction[j] = -local_tangent_dir; + // do not allow switching from static friction to dynamic friction + // it causes cg to explode + if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false) + { + friction.m_static[j] = false; + friction.m_impulse[j] = -accumulated_normal*c->m_c3; + } + else + { + friction.m_static[j] = true; + friction.m_impulse[j] = tangent_norm; + } + } + else + { + friction.m_released[j] = true; + friction.m_static[j] = false; + friction.m_impulse[j] = 0; + friction.m_direction[j] = btVector3(0,0,0); + } + friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt; + friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j]; + + // the incremental impulse applied to rb in the tangential direction + btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); + + + // dv = new_impulse + accumulated velocity change in previous CG iterations + // so we have the invariant node->m_v = backupVelocity + dv; + + btScalar dvn = -accumulated_normal * c->m_c2/m_dt; + + // the following is equivalent + /* + btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)]; + btScalar dvn = dv.dot(cti.m_normal); + */ + + constraint.m_value[j] = dvn; + + // the incremental impulse: + // in the normal direction it's the normal component of "impulse" + // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration + impulse = impulse_normal + incremental_tangent; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse, c->m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + if (incremental_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); + const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2)); + } + } + } + } + } + } + } +} + +void btDeformableContactProjection::setConstraints() +{ + BT_PROFILE("setConstraints"); + // set Dirichlet constraint + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + btAlignedObjectArray<DeformableContactConstraint> c; + c.reserve(3); + c.push_back(DeformableContactConstraint(btVector3(1,0,0))); + c.push_back(DeformableContactConstraint(btVector3(0,1,0))); + c.push_back(DeformableContactConstraint(btVector3(0,0,1))); + m_constraints.insert(psb->m_nodes[j].index, c); + + btAlignedObjectArray<DeformableFrictionConstraint> f; + f.reserve(3); + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + m_frictions.insert(psb->m_nodes[j].index, f); + } + } + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_complementary; + for (int j = 0; j < psb->m_rcontacts.size(); ++j) + { + const btSoftBody::RContact& c = psb->m_rcontacts[j]; + // skip anchor points + if (c.m_node->m_im == 0) + { + continue; + } + + const btSoftBody::sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) + { + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btScalar vel = 0.0; + const btScalar* jac = &c.jacobianData_normal.m_jacobians[0]; + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + for (int j = 0; j < ndof; ++j) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal * vel * m_dt; + } + } + + const btVector3 vb = c.m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + + if (m_constraints.find(c.m_node->index) == NULL) + { + btAlignedObjectArray<DeformableContactConstraint> constraints; + constraints.push_back(DeformableContactConstraint(c)); + m_constraints.insert(c.m_node->index,constraints); + btAlignedObjectArray<DeformableFrictionConstraint> frictions; + frictions.push_back(DeformableFrictionConstraint()); + m_frictions.insert(c.m_node->index,frictions); + } + else + { + // group colinear constraints into one + const btScalar angle_epsilon = 0.015192247; // less than 10 degree + bool merged = false; + btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index]; + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index]; + for (int j = 0; j < constraints.size(); ++j) + { + const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction; + btScalar dot_prod = dirs[0].dot(cti.m_normal); + if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) + { + // group the constraints + constraints[j].append(c); + // push in an empty friction + frictions[j].append(); + merged = true; + break; + } + } + const int dim = 3; + // hard coded no more than 3 constraint directions + if (!merged && constraints.size() < dim) + { + constraints.push_back(DeformableContactConstraint(c)); + frictions.push_back(DeformableFrictionConstraint()); + } + } + } + } + } + } +} + +void btDeformableContactProjection::enforceConstraint(TVStack& x) +{ + const int dim = 3; + for (int index = 0; index < m_constraints.size(); ++index) + { + const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) + { + x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; + for (int j = 0; j < constraints[0].m_direction.size(); ++j) + x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j]; + } + else if (constraints.size() == 2) + { + btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); + btAssert(free_dir.norm() > SIMD_EPSILON) + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + for (int j = 0; j < constraints.size(); ++j) + { + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } + } + } + else + { + x[i].setZero(); + for (int j = 0; j < constraints.size(); ++j) + { + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } + } + } + + // apply friction if the node is not constrained in all directions + if (constraints.size() < 3) + { + for (int f = 0; f < frictions.size(); ++f) + { + const DeformableFrictionConstraint& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + // add the friction constraint + if (friction.m_static[j] == true) + { + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; + x[i] += friction.m_direction[j] * friction.m_dv[j]; + } + } + } + } + } +} + +void btDeformableContactProjection::project(TVStack& x) +{ + const int dim = 3; + for (int index = 0; index < m_constraints.size(); ++index) + { + const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) + { + x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; + } + else if (constraints.size() == 2) + { + btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); + btAssert(free_dir.norm() > SIMD_EPSILON) + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + else + x[i].setZero(); + + // apply friction if the node is not constrained in all directions + if (constraints.size() < 3) + { + bool has_static_constraint = false; + for (int f = 0; f < frictions.size(); ++f) + { + DeformableFrictionConstraint& friction= frictions[f]; + for (int j = 0; j < friction.m_static.size(); ++j) + has_static_constraint = has_static_constraint || friction.m_static[j]; + } + + for (int f = 0; f < frictions.size(); ++f) + { + DeformableFrictionConstraint& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + // clear the old friction force + if (friction.m_static_prev[j] == false) + { + x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j]; + } + + // only add to the rhs if there is no static friction constraint on the node + if (friction.m_static[j] == false) + { + if (!has_static_constraint) + x[i] += friction.m_direction[j] * friction.m_impulse[j]; + } + else + { + // otherwise clear the constraint in the friction direction + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; + } + } + } + } + } +} + +void btDeformableContactProjection::reinitialize(bool nodeUpdated) +{ + btCGProjection::reinitialize(nodeUpdated); + m_constraints.clear(); + m_frictions.clear(); +} + + + diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h new file mode 100644 index 000000000..64d448b5e --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -0,0 +1,50 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_CONTACT_PROJECTION_H +#define BT_CONTACT_PROJECTION_H +#include "btCGProjection.h" +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "LinearMath/btHashMap.h" +class btDeformableContactProjection : public btCGProjection +{ +public: + // map from node index to constraints + btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints; + btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions; + + btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) + : btCGProjection(softBodies, dt) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs + virtual void project(TVStack& x); + + // apply constraints to x in Ax=b + virtual void enforceConstraint(TVStack& x); + + // update the constraints + virtual void update(); + + virtual void setConstraints(); + + virtual void reinitialize(bool nodeUpdated); +}; +#endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h new file mode 100644 index 000000000..270222b7e --- /dev/null +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -0,0 +1,68 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H +#define BT_DEFORMABLE_GRAVITY_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableGravityForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; + btVector3 m_gravity; + + btDeformableGravityForce(const btVector3& g) : m_gravity(g) + { + } + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + + } + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = n.index; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass; + force[id] += scaled_force; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } + + +}; +#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h new file mode 100644 index 000000000..eb4d032bf --- /dev/null +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -0,0 +1,72 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H +#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H + +#include "btSoftBody.h" +#include <LinearMath/btHashMap.h> + +enum btDeformableLagrangianForceType +{ + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2 +}; + +class btDeformableLagrangianForce +{ +public: +// using TVStack = btAlignedObjectArray<btVector3>; + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody *> m_softBodies; + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btDeformableLagrangianForce() + { + } + + virtual ~btDeformableLagrangianForce(){} + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + virtual btDeformableLagrangianForceType getForceType() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } + + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } +}; +#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h new file mode 100644 index 000000000..f97ef0a03 --- /dev/null +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -0,0 +1,119 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_MASS_SPRING_H +#define BT_MASS_SPRING_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMassSpringForce : public btDeformableLagrangianForce +{ +public: +// using TVStack = btDeformableLagrangianForce::TVStack; + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMassSpringForce() + { + } + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btScalar k_damp = psb->m_dampingCoefficient; + btVector3 scaled_force = scale * v_diff * k_damp; + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar kLST = link.Feature::m_material->m_kLST; + btScalar r = link.m_rl; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // elastic force + // explicit elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = dir.normalized(); + btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + btScalar scaled_k_damp = psb->m_dampingCoefficient * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } + +}; + +#endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp new file mode 100644 index 000000000..3b3b62edc --- /dev/null +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -0,0 +1,266 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +/* ====== Overview of the Deformable Algorithm ====== */ + +/* +A single step of the deformable body simulation contains the following main components: +1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. +2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. +3. Then velocities of deformable bodies v_{n+1} are solved in + M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, + by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}. +4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. +5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. +6. Apply position correction to prevent numerical drift. + +The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf + */ + +#include <stdio.h> +#include "btDeformableRigidDynamicsWorld.h" +#include "btDeformableBodySolver.h" +#include "LinearMath/btQuickprof.h" + +void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +{ + BT_PROFILE("internalSingleStepSimulation"); + reinitialize(timeStep); + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve deformable bodies constraints + solveDeformableBodiesConstraints(timeStep); + + afterSolverCallbacks(timeStep); + + integrateTransforms(timeStep); + + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); + + btMultiBodyDynamicsWorld::updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// +} + +void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) +{ + // perform position correction for all constraints + BT_PROFILE("positionCorrection"); + for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) + { + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) + { + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) + { + const btSoftBody::RContact* c = constraint.m_contact[j]; + // skip anchor points + if (c == NULL || c->m_node->m_im == 0) + continue; + const btSoftBody::sCti& cti = c->m_cti; + btVector3 va(0, 0, 0); + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t1[k]; + } + va += c->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t2[k]; + } + va += c->t2 * vel; + } + } + else + { + // The object interacting with deformable node is not supported for position correction + btAssert(false); + } + + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = cti.m_offset; + + // only perform position correction when penetrating + if (dp < 0) + { + if (friction.m_static[j] == true) + { + c->m_node->m_v = va; + } + c->m_node->m_v -= dp * cti.m_normal / dt; + } + } + } + } + } +} + + +void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) +{ + BT_PROFILE("integrateTransforms"); + m_deformableBodySolver->backupVelocity(); + positionCorrection(dt); + btMultiBodyDynamicsWorld::integrateTransforms(dt); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_x = node.m_q + dt * node.m_v; + } + } + m_deformableBodySolver->revertVelocity(); +} + +void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) +{ + m_deformableBodySolver->solveConstraints(timeStep); +} + +void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +{ + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); +} + +void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); +} + +void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) +{ + m_internalTime += timeStep; + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; +} + +void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) +{ + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + clearForces(); + clearMultiBodyForces(); + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + // integrate multibody gravity + btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); + clearForces(); + clearMultiBodyForces(); +} + +void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) +{ + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) +{ + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } +} diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h new file mode 100644 index 000000000..de5eb7ef4 --- /dev/null +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -0,0 +1,142 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H + +#include "btSoftMultiBodyDynamicsWorld.h" +#include "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" +#include "btDeformableBodySolver.h" +#include "btSoftBodyHelpers.h" +#include <functional> +typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; + +class btDeformableBodySolver; +class btDeformableLagrangianForce; +typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; + +class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld +{ + typedef btAlignedObjectArray<btVector3> TVStack; +// using TVStack = btAlignedObjectArray<btVector3>; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + bool m_ownsSolver; + btScalar m_internalTime; + + typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world); + btSolverCallback m_solverCallback; + +protected: + virtual void internalSingleStepSimulation(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(btScalar dt); + + void solveDeformableBodiesConstraints(btScalar timeStep); + +public: + btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) + { + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0, 0, 0); + m_sbi.m_gravity.setValue(0, -10, 0); + + m_sbi.m_sparsesdf.Initialize(); + m_internalTime = 0.0; + } + + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + + virtual ~btDeformableRigidDynamicsWorld() + { + } + + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() + { + return (btMultiBodyDynamicsWorld*)(this); + } + + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const + { + return (const btMultiBodyDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + + void afterSolverCallbacks(btScalar timeStep); + + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } +}; + +#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h new file mode 100644 index 000000000..663731a58 --- /dev/null +++ b/src/BulletSoftBody/btPreconditioner.h @@ -0,0 +1,74 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_PRECONDITIONER_H +#define BT_PRECONDITIONER_H + +class Preconditioner +{ +public: + typedef btAlignedObjectArray<btVector3> TVStack; +// using TVStack = btAlignedObjectArray<btVector3>; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; +}; + +class DefaultPreconditioner : public Preconditioner +{ +public: + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + + } +}; + +class MassPreconditioner : public Preconditioner +{ + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody *>& m_softBodies; +public: + MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i] * m_inv_mass[i]; + } +}; + +#endif /* BT_PRECONDITIONER_H */ diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 58796a88d..86a48bf62 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -110,6 +110,7 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); + m_dampingCoefficient = 1; } // @@ -1757,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) } } -// void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } - n.m_v += deltaV; - n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); - m_ndbvt.update(n.m_leaf, - vol, - n.m_v * m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if (!m_fdbvt.empty()) - { - for (int i = 0; i < m_faces.size(); ++i) - { - Face& f = m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - vol, - v * m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if (m_pose.m_bframe && (m_cfg.kMT > 0)) - { - const btMatrix3x3 posetrs = m_pose.m_rot; - for (int i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - if (n.m_im > 0) - { - const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; - n.m_x = Lerp(n.m_x, x, m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); + int i, ni; + + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) + { + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + if (n.m_im > 0) + { + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); } + // void btSoftBody::solveConstraints() { @@ -2261,18 +2262,45 @@ btVector3 btSoftBody::evaluateCom() const return (com); } -// bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // 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 + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); +} +// +bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, - btSoftBody::sCti& cti) const + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - // 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 + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( @@ -2280,13 +2308,14 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, shp, nrm, margin); - if (dst < 0) + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); - return (true); + cti.m_offset = dst; } + if (dst < 0) + return true; return (false); } @@ -2774,6 +2803,14 @@ void btSoftBody::dampClusters() } } +void btSoftBody::setSpringStiffness(btScalar k) +{ + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } +} + // void btSoftBody::Joint::Prepare(btScalar dt, int) { @@ -3252,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; + case fCollision::SDF_RD: + { + btSoftColliders::CollideSDF_RD docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); + + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; + docollide.m_colObj1Wrap = pcoWrap; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); + } + break; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 9b35b799d..f2934c94a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -26,7 +26,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "btSparseSDF.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" - +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" //#ifdef BT_USE_DOUBLE_PRECISION //#define btRigidBodyData btRigidBodyDoubleData //#define btRigidBodyDataName "btRigidBodyDoubleData" @@ -159,6 +160,7 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft + SDF_RD = 0x0003, ///DF based rigid vs deformable SVSmask = 0x0030, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -257,6 +259,7 @@ public: btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data int m_battach : 1; // Attached + int index; }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -300,6 +303,13 @@ public: btScalar m_c2; // ima*dt btScalar m_c3; // Friction btScalar m_c4; // Hardness + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; }; /* SContact */ struct SContact @@ -704,6 +714,7 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision @@ -735,6 +746,11 @@ public: { return m_worldInfo; } + + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) @@ -991,7 +1007,8 @@ public: btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -1005,6 +1022,7 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); + void setSpringStiffness(btScalar k); void applyForces(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); @@ -1015,7 +1033,7 @@ public: static vsolver_t getSolver(eVSolver::_ solver); virtual int calculateSerializeBufferSize() const; - + ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 7efe514f3..80545ebad 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -25,7 +25,41 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include <string.h> //for memset +#include <cmath> +static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} +static btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} // // btSymMatrix // @@ -298,6 +332,46 @@ static inline btMatrix3x3 Diagonal(btScalar x) m[2] = btVector3(0, 0, x); return (m); } + +static inline btMatrix3x3 Diagonal(const btVector3& v) +{ + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; +} + +static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1,u1,ndof); + btScalar a12 = Dot(v1,u2,ndof); + btScalar a13 = Dot(v1,u3,ndof); + + btScalar a21 = Dot(v2,u1,ndof); + btScalar a22 = Dot(v2,u2,ndof); + btScalar a23 = Dot(v2,u3,ndof); + + btScalar a31 = Dot(v3,u1,ndof); + btScalar a32 = Dot(v3,u2,ndof); + btScalar a33 = Dot(v3,u3,ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + + // static inline btMatrix3x3 Add(const btMatrix3x3& a, const btMatrix3x3& b) @@ -854,10 +928,62 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // - // CollideSDF_RS + // CollideSDF_RD // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RD : btDbvt::ICollide { void Process(const btDbvtNode* leaf) { @@ -869,34 +995,75 @@ struct btSoftColliders const btScalar m = n.m_im > 0 ? dynmargin : stamargin; btSoftBody::RContact c; - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } + if (!n.m_battach) + { + // check for collision at x_{n+1}^* + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_node = &n; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_q - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + if (m_rigidBody) + m_rigidBody->activate(); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btScalar dt = psb->m_sst.sdt; + btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_rcontacts.push_back(c); + } + } } } btSoftBody* psb; diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h index dcf508265..405475529 100644 --- a/src/BulletSoftBody/btSoftBodySolvers.h +++ b/src/BulletSoftBody/btSoftBodySolvers.h @@ -35,7 +35,8 @@ public: CL_SOLVER, CL_SIMD_SOLVER, DX_SOLVER, - DX_SIMD_SOLVER + DX_SIMD_SOLVER, + DEFORMABLE_SOLVER }; protected: |