AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

clean up code

Browse Source
This commit is contained in:
Chuyuan Fu 2020-08-12 10:13:49 -07:00
parent c5fc913293
commit 25aa0f5669
8 changed files with 23 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -35,9 +35,6 @@
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "../src/LinearMath/btScalar.h"
///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
@ -76,7 +73,7 @@ public:
void resetCamera()
{
float dist = 0.2;
float dist = 0.3;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0, -0.1, 0};
@ -205,9 +202,9 @@ void GraspDeformable::initPhysics()
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_maxPickingForce =100;
m_maxPickingForce = 0.001;
// build a gripper
if(0)
if(1)
{
bool damping = true;
bool gyro = false;
@ -257,7 +254,6 @@ void GraspDeformable::initPhysics()
}
//create a ground
if(0)
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
groundShape->setMargin(0.001);
@ -265,7 +261,7 @@ void GraspDeformable::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -5, 0));
groundTransform.setOrigin(btVector3(0, -5.1, 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.);
@ -287,22 +283,6 @@ void GraspDeformable::initPhysics()
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
if(1)
{
bool gyro = false;
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(10, 5, 10);
btVector3 baseHalfExtents(10, 5, 10);
btVector3 basepos(0, -5, 0);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basepos, linkHalfExtents, baseHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
}
// create a soft block
if (0)
{
@ -358,12 +338,12 @@ void GraspDeformable::initPhysics()
{
bool onGround = false;
const btScalar s = .05;
const btScalar h = 0.01;
const btScalar h = -0.02;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
2,2,
10,10,
0, true);
if (onGround)
@ -376,109 +356,19 @@ void GraspDeformable::initPhysics()
0, true);
psb->getCollisionShape()->setMargin(0.001);
psb->generateBendingConstraints(2);
psb->setTotalMass(0.02);
psb->setTotalMass(0.01);
psb->setSpringStiffness(10);
psb->setDampingCoefficient(0.05);
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;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);
btScalar elastic_stiffness = 0.2;
btScalar damping_stiffness = 0.01;
bool not_damp_all_directions = true;
btScalar bending_stiffness = 0.2;
psb->setSpringStiffness(elastic_stiffness);
btDeformableLagrangianForce* springForce =
new btDeformableMassSpringForce(elastic_stiffness,
damping_stiffness, not_damp_all_directions,bending_stiffness);
getDeformableDynamicsWorld()->addForce(psb, springForce);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
}
if(0){
btSoftBody* psb = NULL;
std::vector<tinyobj::shape_t> shapes;
tinyobj::attrib_t attribute;
char absolute_path[1024];
b3BulletDefaultFileIO fileio;
fileio.findResourcePath("dang_sticky_rice_chips_full_sim.obj", absolute_path, 1024);
std::string err = tinyobj::LoadObj(attribute, shapes, absolute_path, "", &fileio);
if (!shapes.empty())
{
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices;
btAlignedObjectArray<int> indices;
for (int i = 0; i < attribute.vertices.size(); i++)
{
vertices.push_back(attribute.vertices[i]);
}
for (int i = 0; i < shape.mesh.indices.size(); i++)
{
indices.push_back(shape.mesh.indices[i].vertex_index);
}
int numTris = shape.mesh.indices.size() / 3;
if (numTris > 0)
{
psb = btSoftBodyHelpers::CreateFromTriMesh( getDeformableDynamicsWorld()->getWorldInfo(), &vertices[0], &indices[0], numTris);
btAssert(psb!=nullptr);
}
btScalar elastic_stiffness = 0.2;
btScalar damping_stiffness = 0.01;
bool not_damp_all_directions = true;
btScalar bending_stiffness = 0.2;
btDeformableLagrangianForce* springForce =
new btDeformableMassSpringForce(elastic_stiffness,
damping_stiffness, not_damp_all_directions,bending_stiffness);
psb->generateBendingConstraints(3);
getDeformableDynamicsWorld()->addForce(psb, springForce);
m_forces.push_back(springForce);
btVector3 gravity =getDeformableDynamicsWorld()->getGravity();
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity *0.2);
getDeformableDynamicsWorld()->addForce(psb, gravityForce);
m_forces.push_back(gravityForce);
btScalar friction = 1;
psb->m_cfg.kDF = friction;
// turn on the collision flag for deformable
// collision between deformable and rigid
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
// turn on face contact for multibodies
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
/// turn on face contact for rigid body
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
// collion between deformable and deformable and self-collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
psb->setCollisionFlags(0);
psb->setTotalMass(0.02);
psb->setSelfCollision(false);
btScalar repulsionStiffness = 0.2;
psb->setSpringStiffness(repulsionStiffness);
psb->initializeFaceTree();
}
// psb->scale(btVector3(1, 1, 1));
psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5));
psb->translate(btVector3(0,1,0));
psb->getCollisionShape()->setMargin(0.005);
psb->getCollisionShape()->setUserPointer(psb);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
getDeformableDynamicsWorld()->addSoftBody(psb);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

1
src/BulletSoftBody/btConjugateResidual.h
View File

@ -39,7 +39,6 @@ public:
// return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
{
verbose = true;
BT_PROFILE("CRSolve");
btAssert(x.size() == b.size());
reinitialize(b);

2
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -18,7 +18,7 @@
#include "LinearMath/btQuickprof.h"
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
: m_softBodies(softBodies), m_projection(softBodies, backup_v), m_backupVelocity(backup_v), m_implicit(false)
: m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
{
m_massPreconditioner = new MassPreconditioner(m_softBodies);
m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);

28
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -32,17 +32,6 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
// std::cout <<"before solve deformable constraints "<<std::endl;
btSoftBody* psb = m_softBodies[0];
for(int i = 0; i<psb->m_faces.size(); i++){
// std::cout<<"face "<<i<<": ";
const btSoftBody::Face& face =psb->m_faces[i];
btVector3 vel = btVector3(0,0,0);
for(int j = 0; j<3; j++){
vel+= face.m_n[j]->m_v* face.m_pcontact[j];
}
// std::cout<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl;
}
BT_PROFILE("solveDeformableConstraints");
if (!m_implicit)
{
@ -59,25 +48,12 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
m_objective->addLagrangeMultiplier(m_dv, x);
m_objective->m_preconditioner->reinitialize(true);
computeStep(x, rhs);
m_objective->m_projection.checkConstraints(x);
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i] = x[i];
}
}
updateVelocity();
// std::cout <<"after solve deformable constraints "<<std::endl;
btSoftBody* psb = m_softBodies[0];
for(int i = 0; i<psb->m_faces.size(); i++){
// std::cout<<"face "<<i<<": ";
const btSoftBody::Face& face =psb->m_faces[i];
btVector3 vel = btVector3(0,0,0);
for(int j = 0; j<3; j++){
vel+=face.m_n[j]->m_v* face.m_pcontact[j];
}
// std::cout<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl;
}
}
else
{
@ -307,10 +283,6 @@ void btDeformableBodySolver::updateVelocity()
else
{
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
// std::cout<<"node "<<j<<", back up vel "<< m_backupVelocity[counter][0] <<" "<< m_backupVelocity[counter][1]<<" "<<m_backupVelocity[counter][2]<<std::endl;
// std::cout<<" , dv: "<< m_dv[counter][0]<<" "<< m_dv[counter][1]<<" "<< m_dv[counter][2]<<" "<<std::endl;
// std::cout<<", split v: "<<psb->m_nodes[j].m_splitv[0]<<" "<<psb->m_nodes[j].m_splitv[1]<<" "<<psb->m_nodes[j].m_splitv[2]<<" "<<std::endl;
}
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;

17
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -495,8 +495,6 @@ void btDeformableContactProjection::setLagrangeMultiplier()
m_lagrangeMultipliers.push_back(lm);
}
if( m_faceRigidConstraints[i].size()==0){
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
if (!m_nodeRigidConstraints[i][j].m_binding)
@ -522,7 +520,6 @@ void btDeformableContactProjection::setLagrangeMultiplier()
lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
}
m_lagrangeMultipliers.push_back(lm);
}
}
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
@ -536,16 +533,6 @@ void btDeformableContactProjection::setLagrangeMultiplier()
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
LagrangeMultiplier lm;
lm.m_num_nodes = 3;
lm.m_vals[0] = lm.m_vals[1] = lm.m_vals[2] = 0;
btVector3 face_vel = btVector3(0,0,0);
btVector3 backup_face_vel = btVector3(0,0,0);
for(int kk = 0; kk<3; kk++){
face_vel += (face->m_n[kk]->m_v + face->m_n[kk]->m_splitv)* bary[kk];
backup_face_vel += m_backupVelocity[face->m_n[kk]->index] * bary[kk];
}
btVector3 dvel = face_vel - backup_face_vel;
for (int k = 0; k < 3; ++k)
{
@ -560,16 +547,12 @@ void btDeformableContactProjection::setLagrangeMultiplier()
lm.m_dirs[0] = btVector3(1, 0, 0);
lm.m_dirs[1] = btVector3(0, 1, 0);
lm.m_dirs[2] = btVector3(0, 0, 1);
lm.m_vals[0] = dvel[0];
lm.m_vals[1] = dvel[1];
lm.m_vals[2] = dvel[2];
}
else
{
face->m_pcontact[3] = 0;
lm.m_num_constraints = 1;
lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
lm.m_vals[0] = dvel.dot(m_faceRigidConstraints[i][j].m_normal);
}
m_lagrangeMultipliers.push_back(lm);
}

6
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -32,7 +32,6 @@ struct LagrangeMultiplier
btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes
btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints;
int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes;
btScalar m_vals[3];
};
class btDeformableContactProjection
@ -40,7 +39,6 @@ class btDeformableContactProjection
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody*>& m_softBodies;
const TVStack& m_backupVelocity;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
@ -66,8 +64,8 @@ public:
bool m_useStrainLimiting;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backupVelocity)
: m_softBodies(softBodies), m_backupVelocity(backupVelocity)
btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
: m_softBodies(softBodies)
{
}

30
src/BulletSoftBody/btSoftBody.cpp
View File

@ -2860,6 +2860,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
}
#endif
// collision detection using x*
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_q);
@ -2871,7 +2872,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
if (dst >= 0)
return false;
// use the contact position of the previous collision
// Use consistent barycenter to recalculate distance.
#ifdef CACHE_PREV_COLLISION
if (f.m_pcontact[3] != 0)
{
@ -2881,37 +2882,22 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results);
cti.m_colObj = colObjWrap->getCollisionObject();
// std::cout<<"****** distance1 "<<results.distance<<std::endl;
// std::cout<<"normal "<<results.normal[0]<<" "<<results.normal[1]<<" "<<results.normal[2]<<std::endl;
// std::cout<<"point is at "<<contact_point[0]<<" "<<contact_point[1]<<" "<<contact_point[2]<<std::endl;
// std::cout<<"three vertices: "<<std::endl;
// std::cout<<f.m_n[0]->m_x[0]<<" "<<f.m_n[0]->m_x[1]<<" "<<f.m_n[0]->m_x[2]<<" "<<std::endl;
// std::cout<<f.m_n[1]->m_x[0]<<" "<<f.m_n[1]->m_x[1]<<" "<<f.m_n[1]->m_x[2]<<" "<<std::endl;
// std::cout<<f.m_n[2]->m_x[0]<<" "<<f.m_n[2]->m_x[1]<<" "<<f.m_n[2]->m_x[2]<<" "<<std::endl;
dst = results.distance;
cti.m_normal = results.normal;
cti.m_offset = dst;
// use face contact to redo collision detection
//point-convex CD
wtr = colObjWrap->getWorldTransform();
btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
triangle_transform.setOrigin(f.m_n[0]->m_x);
btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
dst = results.distance - csh->getMargin() - margin;
// std::cout<<"contact point 1 "<<results.witnesses[0][0]<< " "<<results.witnesses[0][1]<< " "<<results.witnesses[0][2]<< " "<<std::endl;
// std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl;
// std::cout<<"result distance "<<results.distance<<std::endl;
// std::cout<< csh->getMargin() <<" "<<margin<<std::endl;
// std::cout<<"****** distance 2"<<dst<<std::endl;
// std::cout<<"normal "<< results.normal[0]<< " "<<results.normal[1]<< " "<<results.normal[2]<<std::endl;
return true;
}
#endif
//
// Use triangle-convex CD.
wtr = colObjWrap->getWorldTransform();
btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
triangle_transform.setOrigin(f.m_n[0]->m_x);
@ -2923,18 +2909,12 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
f.m_pcontact[i] = bary[i];
dst = results.distance - csh->getMargin() - margin;
// std::cout<<"contact point 1 "<<contact_point[0]<< " "<<contact_point[1]<< " "<<contact_point[2]<< " "<<std::endl;
// std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl;
// std::cout<<"result distance "<<results.distance<<std::endl;
// std::cout<< csh->getMargin() <<" "<<margin<<std::endl;
// std::cout<<"distance "<<dst<<std::endl;
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = results.normal;
cti.m_offset = dst;
return true;
}
//
void btSoftBody::updateNormals()
{
const btVector3 zv(0, 0, 0);

8
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1766,7 +1766,6 @@ struct btSoftColliders
btVector3 bary;
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
{
// f.m_pcontact[3] = 1;
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
// todo: collision between multibody and fixed deformable face will be missed.
@ -1839,11 +1838,8 @@ struct btSoftColliders
psb->m_faceRigidContacts.push_back(c);
}
}
else
{
f.m_pcontact[3] = 0;
}
//always mark caching contact as false
// Set caching barycenters to be false after collision detection.
// Only turn on when contact is static.
f.m_pcontact[3] = 0;
}
btSoftBody* psb;