AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Added a new friction slope example (ground is rotating). Fixed a problem in the reduced deformable's friction (now clamp the maximum tangential impulse based on the tangential velocity impulse at the current iteration, not the total tangential velocity at the beginning)

Browse Source
This commit is contained in:
jingyuc 2021-09-06 17:56:32 -04:00
parent df07b42318
commit 3a84b05058
8 changed files with 362 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/CMakeLists.txt
View File

@ -394,6 +394,8 @@ SET(BulletExampleBrowser_SRCS
../ReducedDeformableDemo/ModeVisualizer.h
../ReducedDeformableDemo/FreeFall.cpp
../ReducedDeformableDemo/FreeFall.h
../ReducedDeformableDemo/FrictionSlope.cpp
../ReducedDeformableDemo/FrictionSlope.h
../ReducedDeformableDemo/ReducedCollide.cpp
../ReducedDeformableDemo/ReducedCollide.h
../ReducedDeformableDemo/ReducedGrasp.cpp

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -75,6 +75,7 @@
#include "../ReducedDeformableDemo/ModeVisualizer.h"
#include "../ReducedDeformableDemo/BasicTest.h"
#include "../ReducedDeformableDemo/FreeFall.h"
#include "../ReducedDeformableDemo/FrictionSlope.h"
#include "../ReducedDeformableDemo/ReducedCollide.h"
#include "../ReducedDeformableDemo/ReducedGrasp.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
@ -226,6 +227,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Reduced Free Fall", "Free fall ground contact test for the reduced deformable model", ReducedFreeFallCreateFunc),
ExampleEntry(1, "Reduced Collision Test", "Collision between a reduced block and the a rigid block", ReducedCollideCreateFunc),
ExampleEntry(1, "Reduced Grasp", "Grasp a reduced deformable block", ReducedGraspCreateFunc),
ExampleEntry(1, "Reduced Friction Slope", "Grasp a reduced deformable block", FrictionSlopeCreateFunc),
// ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),
#ifdef INCLUDE_CLOTH_DEMOS

107
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -79,8 +79,6 @@ public:
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(btVector3(0,10,0));
// startTransform.setOrigin(btVector3(0,4,2.8));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
}
@ -89,16 +87,6 @@ public:
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
// btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
// std::cout << "rigid_array size=" << deformableWorld->getNonStaticRigidBodies().size() << '\n';
// for (int i = 0; i < deformableWorld->getNonStaticRigidBodies().size(); ++i)
// {
// btRigidBody* rb = deformableWorld->getNonStaticRigidBodies()[i];
// std::cout << "end of timestep, rigid_vel: " << rb->getLinearVelocity()[0] << '\t'
// << rb->getLinearVelocity()[1] << '\t'
// << rb->getLinearVelocity()[2] << '\n';
// }
}
virtual void renderScene()
@ -206,7 +194,7 @@ void FreeFall::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
// groundTransform.setOrigin(btVector3(0, 0, 6));
@ -216,70 +204,6 @@ void FreeFall::initPhysics()
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
// {
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(2), btScalar(0.5), btScalar(1)));
// m_collisionShapes.push_back(groundShape);
// btTransform groundTransform;
// groundTransform.setIdentity();
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setOrigin(btVector3(0, 2, 3.1));
// // groundTransform.setOrigin(btVector3(0, 0, 6));
// // groundTransform.setOrigin(btVector3(0, -50, 0));
// {
// btScalar mass(0.);
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// }
// }
// {
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(2), btScalar(0.5), btScalar(1)));
// m_collisionShapes.push_back(groundShape);
// btTransform groundTransform;
// groundTransform.setIdentity();
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setOrigin(btVector3(0, 2, -3.1));
// // groundTransform.setOrigin(btVector3(0, 0, 6));
// // groundTransform.setOrigin(btVector3(0, -50, 0));
// {
// btScalar mass(0.);
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// }
// }
// {
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(1), btScalar(0.5), btScalar(2)));
// m_collisionShapes.push_back(groundShape);
// btTransform groundTransform;
// groundTransform.setIdentity();
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setOrigin(btVector3(2, 2, 0));
// // groundTransform.setOrigin(btVector3(0, 0, 6));
// // groundTransform.setOrigin(btVector3(0, -50, 0));
// {
// btScalar mass(0.);
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// }
// }
// {
// // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(1), btScalar(0.5), btScalar(2)));
// m_collisionShapes.push_back(groundShape);
// btTransform groundTransform;
// groundTransform.setIdentity();
// // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
// groundTransform.setOrigin(btVector3(-2, 2, 0));
// // groundTransform.setOrigin(btVector3(0, 0, 6));
// // groundTransform.setOrigin(btVector3(0, -50, 0));
// {
// btScalar mass(0.);
// createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// }
// }
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
@ -293,35 +217,6 @@ void FreeFall::initPhysics()
Ctor_RbUpStack();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
// {
// SliderParams slider("Young's Modulus", &E);
// slider.m_minVal = 0;
// slider.m_maxVal = 2000;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Poisson Ratio", &nu);
// slider.m_minVal = 0.05;
// slider.m_maxVal = 0.49;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Mass Damping", &damping_alpha);
// slider.m_minVal = 0;
// slider.m_maxVal = 1;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
// {
// SliderParams slider("Stiffness Damping", &damping_beta);
// slider.m_minVal = 0;
// slider.m_maxVal = 0.1;
// if (m_guiHelper->getParameterInterface())
// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
// }
}
void FreeFall::exitPhysics()

286
examples/ReducedDeformableDemo/FrictionSlope.cpp Normal file
View File

@ -0,0 +1,286 @@
/*
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 "FrictionSlope.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h"
#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
// static btScalar E = 50;
// static btScalar nu = 0.3;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static btScalar COLLIDING_VELOCITY = 0;
static int start_mode = 6;
static int num_modes = 1;
class FrictionSlope : public CommonDeformableBodyBase
{
public:
FrictionSlope(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{}
virtual ~FrictionSlope()
{
}
void initPhysics();
void exitPhysics();
// TODO: disable pick force, non-interactive for now.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 20;
float pitch = -20;
float yaw = 90;
float targetPos[3] = {0, 0, 0.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 10;
// btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
}
void createGround()
{
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
groundTransform.setOrigin(btVector3(0, 0, 0));
btScalar mass(1e6);
btRigidBody* ground = createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
// ground->setFriction(1);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
// btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag);
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
}
// static int num = 0;
for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
{
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
}
}
}
}
};
namespace FrictionSlopeHelper
{
void groundMotion(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
btRigidBody* ground = rbs[0];
btAssert(ground->getMass() > 1e5);
btScalar start_time = 2;
btScalar end_time = 8;
btScalar start_angle = 0;
btScalar end_angle = SIMD_PI / 6;
btScalar current_angle = 0;
btScalar turn_speed = (end_angle - start_angle) / (end_time - start_time);
if (time >= start_time)
{
current_angle = (time - start_time) * turn_speed;
if (time > end_time)
{
current_angle = end_angle;
turn_speed = 0;
}
}
else
{
current_angle = start_angle;
turn_speed = 0;
}
btTransform groundTransform;
groundTransform.setIdentity();
// groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), current_angle));
ground->setCenterOfMassTransform(groundTransform);
ground->setLinearVelocity(btVector3(0, 0, 0));
ground->setAngularVelocity(btVector3(0, 0, 0));
}
};
void FrictionSlope::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();
btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
btVector3 gravity = btVector3(0, -10, 0);
reducedSoftBodySolver->setGravity(gravity);
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string filepath("../../../examples/SoftDemo/beam/");
std::string filename = filepath + "mesh.vtk";
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
rsb->setReducedModes(start_mode, num_modes, rsb->m_nodes.size());
btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
rsb->transform(init_transform);
rsb->setStiffnessScale(10);
rsb->setDamping(damping_alpha, damping_beta);
rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
rsb->m_cfg.kDF = 0;
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
createGround();
// add a few rigid bodies
// Ctor_RbUpStack();
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(true);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void FrictionSlope::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
removePickingConstraint();
//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 forces
for (int j = 0; j < m_forces.size(); j++)
{
btDeformableLagrangianForce* force = m_forces[j];
delete force;
}
m_forces.clear();
//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* FrictionSlopeCreateFunc(struct CommonExampleOptions& options)
{
return new FrictionSlope(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/FrictionSlope.h Normal file
View File

@ -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 _FRICTION_SLOPE_H
#define _FRICTION_SLOPE_H
class CommonExampleInterface* FrictionSlopeCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_FREE_FALL_H

35
examples/ReducedDeformableDemo/ReducedGrasp.cpp
View File

@ -97,16 +97,18 @@ public:
}
}
}
static void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
};
void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
{
btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
if (rbs.size()<2)
return;
btRigidBody* rb0 = rbs[0];
// btScalar pressTime = 0.9;
btScalar pressTime = 1.5;
btScalar pressTime = 1;
btScalar liftTime = 2.5;
btScalar shiftTime = 3.5;
btScalar holdTime = 4.5*1000;
@ -116,8 +118,8 @@ void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0.5,3,4);
btVector3 initialTranslationRight = btVector3(0.5,3,-4);
btVector3 initialTranslationLeft = btVector3(0,3,3); // inner face has z=2
btVector3 initialTranslationRight = btVector3(0,3,-3); // inner face has z=-2
btVector3 pinchVelocityLeft = btVector3(0,0,-2);
btVector3 pinchVelocityRight = btVector3(0,0,2);
btVector3 liftVelocity = btVector3(0,5,0);
@ -133,6 +135,7 @@ void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
}
else if (time < liftTime)
{
exit(1);
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
@ -233,7 +236,7 @@ void ReducedGrasp::initPhysics()
// create volumetric reduced deformable body
{
std::string filepath("../../../examples/SoftDemo/");
std::string filepath("../../../examples/SoftDemo/beam/");
std::string filename = filepath + "mesh.vtk";
btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
@ -242,9 +245,13 @@ void ReducedGrasp::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
// rsb->scale(btVector3(1, 1, 1));
rsb->rotate(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->translate(btVector3(0, 2, 0)); //TODO: add back translate and scale
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(10);
rsb->setDamping(damping_alpha, damping_beta);
@ -256,7 +263,7 @@ void ReducedGrasp::initPhysics()
rsb->m_sleepingThreshold = 0;
btSoftBodyHelpers::generateBoundaryFaces(rsb);
rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
@ -275,6 +282,16 @@ void ReducedGrasp::initPhysics()
createGrip();
// {
// float mass = 10;
// btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
// btTransform startTransform;
// startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0,4,0));
// btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
// rb1->setLinearVelocity(btVector3(0, 0, 0));
// }
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));

36
src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
View File

@ -52,8 +52,8 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
m_appliedTangentImpulse = 0;
m_rhs = 0;
m_cfm = 0;
m_erp = btScalar(0.2); // TODO: add parameter handle
m_friction = btScalar(0);
m_erp = infoGlobal.m_deformable_erp;
m_friction = infoGlobal.m_friction;
btRigidBody* rb = m_contact->m_cti.m_colObj ? (btRigidBody*)btRigidBody::upcast(m_contact->m_cti.m_colObj) : nullptr;
if (!rb)
@ -72,6 +72,10 @@ void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solv
m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);
m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
m_linearComponentTangent = -m_contactTangent * m_solverBody->internalGetInvMass();
btVector3 torqueAxisTangent = -m_relPosA.cross(m_contactNormalA);
m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
}
btVector3 btReducedDeformableRigidContactConstraint::getVa() const
@ -109,11 +113,11 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
btVector3 deltaV_rel = deltaVa - deltaVb;
btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
if (!m_collideStatic)
{
// if (!m_collideStatic)
// {
std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
}
// }
// get the normal impulse to be applied
btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor;
@ -155,23 +159,28 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
{
// trial velocity change
// deltaImpulse_tangent = m_friction * deltaImpulse;
// btScalar deltaV_tangent_trial = deltaImpulse_tangent * m_impulseFactorTangent;
// btScalar deltaV_tangent_trial = deltaImpulse_tangent * m_tangentImpulseFactor;
btScalar total_deltaV_rel_normal = m_appliedNormalImpulse * m_normalImpulseFactor;
btScalar deltaV_tangent_trial = m_friction * total_deltaV_rel_normal;
btScalar deltaV_tangent_trial = abs(m_friction * total_deltaV_rel_normal);
// initial relative tangential veloity magnitude
btScalar v_rel_tangent = abs((Va - getVb()).dot(m_contactTangent));
btScalar deltaV_rel_tangent = deltaV_rel.dot(m_contactTangent);
std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << "\n";
btScalar v_rel_tangent_budget = v_rel_tangent - deltaV_rel_tangent;
// if the trial velocity change is greater than the current tangential relative velocity
if (deltaV_tangent_trial > v_rel_tangent)
if (deltaV_tangent_trial > v_rel_tangent_budget)
{
deltaImpulse_tangent = v_rel_tangent / m_tangentImpulseFactor;
std::cout << "clamped!!\n";
deltaImpulse_tangent = v_rel_tangent_budget / m_tangentImpulseFactor;
}
else
{
std::cout << "ok. not too big\n";
deltaImpulse_tangent = deltaV_tangent_trial / m_tangentImpulseFactor;
}
}
btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
@ -190,11 +199,15 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
{
m_appliedTangentImpulse = sum;
}
std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n";
std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
}
// get the total impulse vector
btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
btVector3 impulse_tangent = -deltaImpulse_tangent * m_contactTangent;
btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
btVector3 impulse = impulse_normal + impulse_tangent;
applyImpulse(impulse);
@ -221,6 +234,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
// m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
std::cout << "after\n";
std::cout << "rigid impulse applied!!\n";

3
src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
View File

@ -8,7 +8,7 @@
btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m)
{
m_rigidOnly = false; //! only use rigid frame to debug
m_rigidOnly = true; //! only use rigid frame to debug
// reduced deformable
m_reducedModel = true;
@ -208,7 +208,6 @@ void btReducedSoftBody::updateExternalForceProjectMatrix(bool initialized)
// m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]);
}
}
// std::cout << "----------------\n";
}
void btReducedSoftBody::endOfTimeStepZeroing()