mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-19 05:20:06 +00:00
Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
Erwin Coumans
commit
2f08938110
@ -206,15 +206,15 @@ void DeformableMultibody::initPhysics()
|
||||
|
||||
psb->getCollisionShape()->setMargin(0.25);
|
||||
psb->generateBendingConstraints(2);
|
||||
psb->setTotalMass(5);
|
||||
psb->setTotalMass(1);
|
||||
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.kDF = 2;
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
psb->setCollisionFlags(0);
|
||||
getDeformableDynamicsWorld()->addSoftBody(psb);
|
||||
|
||||
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);
|
||||
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true);
|
||||
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
|
||||
m_forces.push_back(mass_spring);
|
||||
|
||||
|
||||
@ -219,7 +219,7 @@ void DeformableRigid::initPhysics()
|
||||
psb->setTotalMass(1);
|
||||
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.kDF = .4;
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
getDeformableDynamicsWorld()->addSoftBody(psb);
|
||||
|
||||
|
||||
@ -275,12 +275,12 @@ void GraspDeformable::initPhysics()
|
||||
{
|
||||
char absolute_path[1024];
|
||||
b3BulletDefaultFileIO fileio;
|
||||
fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
|
||||
fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("banana.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("ball.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("single_tet.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("tube.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("tube.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("torus.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024);
|
||||
// fileio.findResourcePath("bread.vtk", absolute_path, 1024);
|
||||
|
||||
@ -231,7 +231,7 @@ void Pinch::initPhysics()
|
||||
btVector3 gravity = btVector3(0, -10, 0);
|
||||
m_dynamicsWorld->setGravity(gravity);
|
||||
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
|
||||
|
||||
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
|
||||
getDeformableDynamicsWorld()->setSolverCallback(dynamics);
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
|
||||
|
||||
@ -425,12 +425,12 @@ public:
|
||||
|
||||
m_robotSim.setGravity(btVector3(0, 0, -10));
|
||||
b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
|
||||
args.m_springElasticStiffness = .1;
|
||||
args.m_springDampingStiffness = .0004;
|
||||
args.m_springBendingStiffness = 1;
|
||||
args.m_frictionCoeff = 1;
|
||||
args.m_springElasticStiffness = 1;
|
||||
args.m_springDampingStiffness = .01;
|
||||
args.m_springBendingStiffness = .1;
|
||||
args.m_frictionCoeff = 10;
|
||||
args.m_useSelfCollision = false;
|
||||
// args.m_useFaceContact = true;
|
||||
args.m_useFaceContact = true;
|
||||
args.m_useBendingSprings = true;
|
||||
args.m_startPosition.setValue(0, 0, 0);
|
||||
args.m_startOrientation.setValue(0, 0, 1, 1);
|
||||
@ -476,7 +476,7 @@ public:
|
||||
revoluteJoint2.m_jointType = ePoint2PointType;
|
||||
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
|
||||
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
|
||||
m_robotSim.setNumSimulationSubSteps(8);
|
||||
m_robotSim.setNumSimulationSubSteps(2);
|
||||
}
|
||||
|
||||
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
|
||||
|
||||
@ -8153,7 +8153,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
|
||||
{
|
||||
spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
|
||||
}
|
||||
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false, spring_bending_stiffness);
|
||||
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
@ -9353,6 +9353,15 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
|
||||
if (deformWorld)
|
||||
{
|
||||
deformWorld->getWorldInfo().m_gravity = grav;
|
||||
for (int i = 0; i < m_data->m_lf.size(); ++i)
|
||||
{
|
||||
btDeformableLagrangianForce* force = m_data->m_lf[i];
|
||||
if (force->getForceType() == BT_GRAVITY_FORCE)
|
||||
{
|
||||
btDeformableGravityForce* gforce = (btDeformableGravityForce*)force;
|
||||
gforce->m_gravity = grav;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,60 @@
|
||||
# Code adapted from https://github.com/araffin/rl-baselines-zoo
|
||||
# it requires stable-baselines to be installed
|
||||
# Colab Notebook: https://colab.research.google.com/drive/1nZkHO4QTYfAksm9ZTaZ5vXyC7szZxC3F
|
||||
# Author: Antonin RAFFIN
|
||||
# MIT License
|
||||
import argparse
|
||||
|
||||
import pybullet_envs
|
||||
|
||||
import gym
|
||||
import numpy as np
|
||||
from stable_baselines import SAC, TD3
|
||||
from stable_baselines.common.noise import NormalActionNoise
|
||||
|
||||
from utils import TimeFeatureWrapper, EvalCallback
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--algo', help='RL Algorithm (Soft Actor-Critic by default)', default='sac',
|
||||
type=str, required=False, choices=['sac', 'td3'])
|
||||
parser.add_argument('--env', type=str, default='HalfCheetahBulletEnv-v0', help='environment ID')
|
||||
parser.add_argument('-n', '--n-timesteps', help='Number of training timesteps', default=int(1e6),
|
||||
type=int)
|
||||
args = parser.parse_args()
|
||||
|
||||
env_id = args.env
|
||||
n_timesteps = args.n_timesteps
|
||||
save_path = '{}_{}'.format(args.algo, env_id)
|
||||
|
||||
# Instantiate and wrap the environment
|
||||
env = TimeFeatureWrapper(gym.make(env_id))
|
||||
|
||||
# Create the evaluation environment and callback
|
||||
eval_env = TimeFeatureWrapper(gym.make(env_id))
|
||||
callback = EvalCallback(eval_env, best_model_save_path=save_path + '_best')
|
||||
|
||||
algo = {
|
||||
'sac': SAC,
|
||||
'td3': TD3
|
||||
}[args.algo]
|
||||
|
||||
n_actions = env.action_space.shape[0]
|
||||
|
||||
# Tuned hyperparameters from https://github.com/araffin/rl-baselines-zoo
|
||||
hyperparams = {
|
||||
'sac': dict(batch_size=256, gamma=0.98, policy_kwargs=dict(layers=[256, 256]),
|
||||
learning_starts=10000, buffer_size=int(2e5), tau=0.01),
|
||||
|
||||
'td3': dict(batch_size=100, policy_kwargs=dict(layers=[400, 300]),
|
||||
learning_rate=1e-3, learning_starts=10000, buffer_size=int(1e6),
|
||||
train_freq=1000, gradient_steps=1000,
|
||||
action_noise=NormalActionNoise(mean=np.zeros(n_actions), sigma=0.1 * np.ones(n_actions)))
|
||||
}[args.algo]
|
||||
|
||||
model = algo('MlpPolicy', env, verbose=1, **hyperparams)
|
||||
model.learn(n_timesteps, callback=callback)
|
||||
|
||||
print("Saving to {}.zip".format(save_path))
|
||||
model.save(save_path)
|
||||
114
examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py
Normal file
114
examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py
Normal file
@ -0,0 +1,114 @@
|
||||
# Code adapted from https://github.com/araffin/rl-baselines-zoo
|
||||
# it requires stable-baselines to be installed
|
||||
# Author: Antonin RAFFIN
|
||||
# MIT License
|
||||
import gym
|
||||
import numpy as np
|
||||
from gym.wrappers import TimeLimit
|
||||
|
||||
from stable_baselines.common.evaluation import evaluate_policy
|
||||
|
||||
|
||||
class TimeFeatureWrapper(gym.Wrapper):
|
||||
"""
|
||||
Add remaining time to observation space for fixed length episodes.
|
||||
See https://arxiv.org/abs/1712.00378 and https://github.com/aravindr93/mjrl/issues/13.
|
||||
|
||||
:param env: (gym.Env)
|
||||
:param max_steps: (int) Max number of steps of an episode
|
||||
if it is not wrapped in a TimeLimit object.
|
||||
:param test_mode: (bool) In test mode, the time feature is constant,
|
||||
equal to zero. This allow to check that the agent did not overfit this feature,
|
||||
learning a deterministic pre-defined sequence of actions.
|
||||
"""
|
||||
def __init__(self, env, max_steps=1000, test_mode=False):
|
||||
assert isinstance(env.observation_space, gym.spaces.Box)
|
||||
# Add a time feature to the observation
|
||||
low, high = env.observation_space.low, env.observation_space.high
|
||||
low, high= np.concatenate((low, [0])), np.concatenate((high, [1.]))
|
||||
env.observation_space = gym.spaces.Box(low=low, high=high, dtype=np.float32)
|
||||
|
||||
super(TimeFeatureWrapper, self).__init__(env)
|
||||
|
||||
if isinstance(env, TimeLimit):
|
||||
self._max_steps = env._max_episode_steps
|
||||
else:
|
||||
self._max_steps = max_steps
|
||||
self._current_step = 0
|
||||
self._test_mode = test_mode
|
||||
|
||||
def reset(self):
|
||||
self._current_step = 0
|
||||
return self._get_obs(self.env.reset())
|
||||
|
||||
def step(self, action):
|
||||
self._current_step += 1
|
||||
obs, reward, done, info = self.env.step(action)
|
||||
return self._get_obs(obs), reward, done, info
|
||||
|
||||
def _get_obs(self, obs):
|
||||
"""
|
||||
Concatenate the time feature to the current observation.
|
||||
|
||||
:param obs: (np.ndarray)
|
||||
:return: (np.ndarray)
|
||||
"""
|
||||
# Remaining time is more general
|
||||
time_feature = 1 - (self._current_step / self._max_steps)
|
||||
if self._test_mode:
|
||||
time_feature = 1.0
|
||||
# Optionnaly: concatenate [time_feature, time_feature ** 2]
|
||||
return np.concatenate((obs, [time_feature]))
|
||||
|
||||
|
||||
class EvalCallback(object):
|
||||
"""
|
||||
Callback for evaluating an agent.
|
||||
|
||||
:param eval_env: (gym.Env) The environment used for initialization
|
||||
:param n_eval_episodes: (int) The number of episodes to test the agent
|
||||
:param eval_freq: (int) Evaluate the agent every eval_freq call of the callback.
|
||||
:param deterministic: (bool)
|
||||
:param best_model_save_path: (str)
|
||||
:param verbose: (int)
|
||||
"""
|
||||
def __init__(self, eval_env, n_eval_episodes=5, eval_freq=10000,
|
||||
deterministic=True, best_model_save_path=None, verbose=1):
|
||||
super(EvalCallback, self).__init__()
|
||||
self.n_eval_episodes = n_eval_episodes
|
||||
self.eval_freq = eval_freq
|
||||
self.best_mean_reward = -np.inf
|
||||
self.deterministic = deterministic
|
||||
self.eval_env = eval_env
|
||||
self.verbose = verbose
|
||||
self.model, self.num_timesteps = None, 0
|
||||
self.best_model_save_path = best_model_save_path
|
||||
self.n_calls = 0
|
||||
|
||||
def __call__(self, locals_, globals_):
|
||||
"""
|
||||
:param locals_: (dict)
|
||||
:param globals_: (dict)
|
||||
:return: (bool)
|
||||
"""
|
||||
self.n_calls += 1
|
||||
self.model = locals_['self']
|
||||
self.num_timesteps = self.model.num_timesteps
|
||||
|
||||
if self.n_calls % self.eval_freq == 0:
|
||||
episode_rewards, _ = evaluate_policy(self.model, self.eval_env, n_eval_episodes=self.n_eval_episodes,
|
||||
deterministic=self.deterministic, return_episode_rewards=True)
|
||||
|
||||
|
||||
mean_reward, std_reward = np.mean(episode_rewards), np.std(episode_rewards)
|
||||
if self.verbose > 0:
|
||||
print("Eval num_timesteps={}, "
|
||||
"episode_reward={:.2f} +/- {:.2f}".format(self.num_timesteps, mean_reward, std_reward))
|
||||
|
||||
if mean_reward > self.best_mean_reward:
|
||||
if self.best_model_save_path is not None:
|
||||
print("Saving best model")
|
||||
self.model.save(self.best_model_save_path)
|
||||
self.best_mean_reward = mean_reward
|
||||
|
||||
return True
|
||||
2
setup.py
2
setup.py
@ -491,7 +491,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
|
||||
|
||||
setup(
|
||||
name='pybullet',
|
||||
version='2.6.3',
|
||||
version='2.6.4',
|
||||
description=
|
||||
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
|
||||
long_description=
|
||||
|
||||
@ -28,6 +28,7 @@ subject to the following restrictions:
|
||||
btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
|
||||
: btCollisionDispatcher(config)
|
||||
{
|
||||
m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads());
|
||||
m_batchUpdating = false;
|
||||
m_grainSize = grainSize; // iterations per task
|
||||
}
|
||||
@ -65,6 +66,10 @@ btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionO
|
||||
manifold->m_index1a = m_manifoldsPtr.size();
|
||||
m_manifoldsPtr.push_back(manifold);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold);
|
||||
}
|
||||
|
||||
return manifold;
|
||||
}
|
||||
@ -121,7 +126,7 @@ struct CollisionDispatcherUpdater : public btIParallelForBody
|
||||
|
||||
void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
|
||||
{
|
||||
int pairCount = pairCache->getNumOverlappingPairs();
|
||||
const int pairCount = pairCache->getNumOverlappingPairs();
|
||||
if (pairCount == 0)
|
||||
{
|
||||
return;
|
||||
@ -136,16 +141,17 @@ void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache*
|
||||
btParallelFor(0, pairCount, m_grainSize, updater);
|
||||
m_batchUpdating = false;
|
||||
|
||||
// reconstruct the manifolds array to ensure determinism
|
||||
m_manifoldsPtr.resizeNoInitialize(0);
|
||||
|
||||
btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
|
||||
for (int i = 0; i < pairCount; ++i)
|
||||
// merge new manifolds, if any
|
||||
for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
|
||||
{
|
||||
if (btCollisionAlgorithm* algo = pairs[i].m_algorithm)
|
||||
btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i];
|
||||
|
||||
for (int j = 0; j < batchManifoldsPtr.size(); ++j)
|
||||
{
|
||||
algo->getAllContactManifolds(m_manifoldsPtr);
|
||||
m_manifoldsPtr.push_back(batchManifoldsPtr[j]);
|
||||
}
|
||||
|
||||
batchManifoldsPtr.resizeNoInitialize(0);
|
||||
}
|
||||
|
||||
// update the indices (used when releasing manifolds)
|
||||
|
||||
@ -30,6 +30,7 @@ public:
|
||||
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE;
|
||||
|
||||
protected:
|
||||
btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr;
|
||||
bool m_batchUpdating;
|
||||
int m_grainSize;
|
||||
};
|
||||
|
||||
@ -139,7 +139,12 @@ public:
|
||||
|
||||
if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
|
||||
{
|
||||
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index);
|
||||
btTransform preTransform = childTrans;
|
||||
if (this->m_compoundColObjWrap->m_preTransform)
|
||||
{
|
||||
preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
|
||||
}
|
||||
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
|
||||
|
||||
btCollisionAlgorithm* algo = 0;
|
||||
bool allocatedAlgorithm = false;
|
||||
|
||||
@ -46,7 +46,7 @@ struct btContactSolverInfoData
|
||||
btScalar m_sor; //successive over-relaxation term
|
||||
btScalar m_erp; //error reduction for non-contact constraints
|
||||
btScalar m_erp2; //error reduction for contact constraints
|
||||
btScalar m_deformable_erp; //error reduction for deformable constraints
|
||||
btScalar m_deformable_erp; //error reduction for deformable constraints
|
||||
btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts
|
||||
btScalar m_frictionERP; //error reduction for friction constraints
|
||||
btScalar m_frictionCFM; //constraint force mixing for friction constraints
|
||||
@ -82,7 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.2);
|
||||
m_erp2 = btScalar(0.2);
|
||||
m_deformable_erp = btScalar(0.);
|
||||
m_deformable_erp = btScalar(0.3);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
|
||||
m_frictionCFM = btScalar(0.);
|
||||
|
||||
@ -186,9 +186,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
|
||||
}
|
||||
|
||||
//set constraints as projections
|
||||
void btDeformableBackwardEulerObjective::setConstraints()
|
||||
void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
m_projection.setConstraints();
|
||||
m_projection.setConstraints(infoGlobal);
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
|
||||
|
||||
@ -79,7 +79,7 @@ public:
|
||||
void updateVelocity(const TVStack& dv);
|
||||
|
||||
//set constraints as projections
|
||||
void setConstraints();
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// update the projections and project the residual
|
||||
void project(TVStack& r)
|
||||
|
||||
@ -228,16 +228,16 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
|
||||
m_objective->reinitialize(nodeUpdated, dt);
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::setConstraints()
|
||||
void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("setConstraint");
|
||||
m_objective->setConstraints();
|
||||
m_objective->setConstraints(infoGlobal);
|
||||
}
|
||||
|
||||
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
|
||||
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("solveContactConstraints");
|
||||
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
|
||||
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
|
||||
return maxSquaredResidual;
|
||||
}
|
||||
|
||||
|
||||
@ -65,7 +65,7 @@ public:
|
||||
virtual void solveDeformableConstraints(btScalar solverdt);
|
||||
|
||||
// solve the contact between deformable and rigid as well as among deformables
|
||||
btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
|
||||
btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// solve the position error between deformable and rigid as well as among deformables;
|
||||
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
|
||||
@ -77,7 +77,7 @@ public:
|
||||
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
|
||||
|
||||
// set up contact constraints
|
||||
void setConstraints();
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
|
||||
virtual void predictMotion(btScalar solverdt);
|
||||
|
||||
@ -15,9 +15,9 @@
|
||||
|
||||
#include "btDeformableContactConstraint.h"
|
||||
/* ================ Deformable Node Anchor =================== */
|
||||
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
|
||||
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
|
||||
: m_anchor(&a)
|
||||
, btDeformableContactConstraint(a.m_cti.m_normal)
|
||||
, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
@ -79,7 +79,7 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
|
||||
return va;
|
||||
}
|
||||
|
||||
btScalar btDeformableNodeAnchorConstraint::solveConstraint()
|
||||
btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
const btSoftBody::sCti& cti = m_anchor->m_cti;
|
||||
btVector3 va = getVa();
|
||||
@ -134,14 +134,14 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
|
||||
}
|
||||
|
||||
/* ================ Deformable vs. Rigid =================== */
|
||||
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
|
||||
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
|
||||
: m_contact(&c)
|
||||
, btDeformableContactConstraint(c.m_cti.m_normal)
|
||||
, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
|
||||
{
|
||||
m_total_normal_dv.setZero();
|
||||
m_total_tangent_dv.setZero();
|
||||
// penetration is non-positive. The magnitude of penetration is the depth of penetration.
|
||||
m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
|
||||
// The magnitude of penetration is the depth of penetration.
|
||||
m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
|
||||
}
|
||||
|
||||
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
|
||||
@ -206,16 +206,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
|
||||
return va;
|
||||
}
|
||||
|
||||
btScalar btDeformableRigidContactConstraint::solveConstraint()
|
||||
btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
const btSoftBody::sCti& cti = m_contact->m_cti;
|
||||
btVector3 va = getVa();
|
||||
btVector3 vb = getVb();
|
||||
btVector3 vr = vb - va;
|
||||
const btScalar dn = btDot(vr, cti.m_normal);
|
||||
const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
|
||||
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
|
||||
btScalar residualSquare = dn*dn;
|
||||
btVector3 impulse = m_contact->m_c0 * vr;
|
||||
btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ;
|
||||
const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
|
||||
btVector3 impulse_tangent = impulse - impulse_normal;
|
||||
btVector3 old_total_tangent_dv = m_total_tangent_dv;
|
||||
@ -256,6 +256,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
|
||||
impulse = impulse_normal + impulse_tangent;
|
||||
// apply impulse to deformable nodes involved and change their velocities
|
||||
applyImpulse(impulse);
|
||||
if (residualSquare < 1e-7)
|
||||
return residualSquare;
|
||||
// apply impulse to the rigid/multibodies involved and change their velocities
|
||||
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
|
||||
{
|
||||
@ -319,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
|
||||
}
|
||||
|
||||
/* ================ Node vs. Rigid =================== */
|
||||
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
|
||||
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
|
||||
: m_node(contact.m_node)
|
||||
, btDeformableRigidContactConstraint(contact)
|
||||
, btDeformableRigidContactConstraint(contact, infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
@ -357,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3&
|
||||
};
|
||||
|
||||
/* ================ Face vs. Rigid =================== */
|
||||
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
|
||||
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal)
|
||||
: m_face(contact.m_face)
|
||||
, btDeformableRigidContactConstraint(contact)
|
||||
, btDeformableRigidContactConstraint(contact, infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
@ -411,19 +413,60 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
|
||||
v1 -= dv * contact->m_weights[1];
|
||||
if (im2 > 0)
|
||||
v2 -= dv * contact->m_weights[2];
|
||||
|
||||
// apply strain limiting to prevent undamped modes
|
||||
btScalar m01 = (btScalar(1)/(im0 + im1));
|
||||
btScalar m02 = (btScalar(1)/(im0 + im2));
|
||||
btScalar m12 = (btScalar(1)/(im1 + im2));
|
||||
|
||||
btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
|
||||
btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
|
||||
btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
|
||||
|
||||
v0 += dv0;
|
||||
v1 += dv1;
|
||||
v2 += dv2;
|
||||
|
||||
btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations);
|
||||
btScalar m01 = (relaxation/(im0 + im1));
|
||||
btScalar m02 = (relaxation/(im0 + im2));
|
||||
btScalar m12 = (relaxation/(im1 + im2));
|
||||
#ifdef USE_STRAIN_RATE_LIMITING
|
||||
// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
|
||||
btScalar p = 0.01;
|
||||
btVector3& x0 = face->m_n[0]->m_x;
|
||||
btVector3& x1 = face->m_n[1]->m_x;
|
||||
btVector3& x2 = face->m_n[2]->m_x;
|
||||
const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1};
|
||||
const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1};
|
||||
btVector3 u[3];
|
||||
btScalar x_diff_dot_u, dn[3];
|
||||
btScalar dt = m_infoGlobal->m_timeStep;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btScalar x_diff_norm = x_diff[i].safeNorm();
|
||||
btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
|
||||
btScalar strainRate = x_diff_norm_new/x_diff_norm;
|
||||
u[i] = v_diff[i];
|
||||
u[i].safeNormalize();
|
||||
if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p))
|
||||
{
|
||||
dn[i] = 0;
|
||||
continue;
|
||||
}
|
||||
x_diff_dot_u = btDot(x_diff[i], u[i]);
|
||||
btScalar s;
|
||||
if (1-p > strainRate)
|
||||
{
|
||||
s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm));
|
||||
}
|
||||
else
|
||||
{
|
||||
s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm));
|
||||
}
|
||||
// x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
|
||||
// strainRate = x_diff_norm_new/x_diff_norm;
|
||||
dn[i] = s - v_diff[i].safeNorm();
|
||||
}
|
||||
btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1]));
|
||||
btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2]));
|
||||
btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1]));
|
||||
#else
|
||||
// apply strain limiting to prevent undamped modes
|
||||
btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
|
||||
btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
|
||||
btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
|
||||
#endif
|
||||
v0 += dv0;
|
||||
v1 += dv1;
|
||||
v2 += dv2;
|
||||
}
|
||||
|
||||
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
|
||||
@ -447,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3&
|
||||
}
|
||||
|
||||
/* ================ Face vs. Node =================== */
|
||||
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
|
||||
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
|
||||
: m_node(contact.m_node)
|
||||
, m_face(contact.m_face)
|
||||
, m_contact(&contact)
|
||||
, btDeformableContactConstraint(contact.m_normal)
|
||||
, btDeformableContactConstraint(contact.m_normal, infoGlobal)
|
||||
{
|
||||
m_total_normal_dv.setZero();
|
||||
m_total_tangent_dv.setZero();
|
||||
@ -487,7 +530,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n
|
||||
return dv * contact->m_weights[2];
|
||||
}
|
||||
|
||||
btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
|
||||
btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
btVector3 va = getVa();
|
||||
btVector3 vb = getVb();
|
||||
|
||||
@ -24,31 +24,33 @@ public:
|
||||
// True if the friction is static
|
||||
// False if the friction is dynamic
|
||||
bool m_static;
|
||||
|
||||
// normal of the contact
|
||||
btVector3 m_normal;
|
||||
|
||||
btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(const btDeformableContactConstraint& other)
|
||||
: m_static(other.m_static)
|
||||
, m_normal(other.m_normal)
|
||||
{
|
||||
|
||||
}
|
||||
btDeformableContactConstraint(){}
|
||||
|
||||
const btContactSolverInfo* m_infoGlobal;
|
||||
|
||||
// normal of the contact
|
||||
btVector3 m_normal;
|
||||
|
||||
btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(){}
|
||||
|
||||
btDeformableContactConstraint(const btDeformableContactConstraint& other)
|
||||
: m_static(other.m_static)
|
||||
, m_normal(other.m_normal)
|
||||
, m_infoGlobal(other.m_infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableContactConstraint(){}
|
||||
|
||||
// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
|
||||
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
|
||||
virtual btScalar solveConstraint() = 0;
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
|
||||
|
||||
// solve the position error by applying an inelastic impulse that changes only the position (not velocity)
|
||||
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
|
||||
@ -79,22 +81,19 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint
|
||||
public:
|
||||
const btSoftBody::Node* m_node;
|
||||
|
||||
btDeformableStaticConstraint(){}
|
||||
|
||||
btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
|
||||
btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableStaticConstraint(){}
|
||||
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
|
||||
: m_node(other.m_node)
|
||||
, btDeformableContactConstraint(other)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual ~btDeformableStaticConstraint(){}
|
||||
|
||||
virtual btScalar solveConstraint()
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
@ -130,14 +129,14 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
|
||||
|
||||
btDeformableNodeAnchorConstraint(){}
|
||||
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
|
||||
|
||||
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
|
||||
btDeformableNodeAnchorConstraint(){}
|
||||
virtual ~btDeformableNodeAnchorConstraint()
|
||||
{
|
||||
}
|
||||
virtual btScalar solveConstraint();
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// todo xuchenhan@
|
||||
@ -169,10 +168,10 @@ public:
|
||||
btVector3 m_total_tangent_dv;
|
||||
btScalar m_penetration;
|
||||
const btSoftBody::DeformableRigidContact* m_contact;
|
||||
|
||||
btDeformableRigidContactConstraint(){}
|
||||
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
|
||||
|
||||
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
|
||||
btDeformableRigidContactConstraint(){}
|
||||
virtual ~btDeformableRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
@ -180,7 +179,7 @@ public:
|
||||
// object A is the rigid/multi body, and object B is the deformable node/face
|
||||
virtual btVector3 getVa() const;
|
||||
|
||||
virtual btScalar solveConstraint();
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
@ -197,11 +196,10 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo
|
||||
public:
|
||||
// the deformable node in contact
|
||||
const btSoftBody::Node* m_node;
|
||||
|
||||
btDeformableNodeRigidContactConstraint(){}
|
||||
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
|
||||
|
||||
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
|
||||
|
||||
btDeformableNodeRigidContactConstraint(){}
|
||||
virtual ~btDeformableNodeRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo
|
||||
{
|
||||
public:
|
||||
const btSoftBody::Face* m_face;
|
||||
btDeformableFaceRigidContactConstraint(){}
|
||||
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
|
||||
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
|
||||
|
||||
btDeformableFaceRigidContactConstraint(){}
|
||||
virtual ~btDeformableFaceRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
@ -263,13 +260,11 @@ public:
|
||||
btVector3 m_total_normal_dv;
|
||||
btVector3 m_total_tangent_dv;
|
||||
|
||||
btDeformableFaceNodeContactConstraint(){}
|
||||
|
||||
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
|
||||
|
||||
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableFaceNodeContactConstraint(){}
|
||||
virtual ~btDeformableFaceNodeContactConstraint(){}
|
||||
|
||||
virtual btScalar solveConstraint();
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
|
||||
@ -17,7 +17,7 @@
|
||||
#include "btDeformableMultiBodyDynamicsWorld.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
|
||||
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
btScalar residualSquare = 0;
|
||||
for (int i = 0; i < numDeformableBodies; ++i)
|
||||
@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
|
||||
for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveConstraint();
|
||||
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveConstraint();
|
||||
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveConstraint();
|
||||
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveConstraint();
|
||||
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
}
|
||||
@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI
|
||||
return residualSquare;
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::setConstraints()
|
||||
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("setConstraints");
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
@ -124,7 +124,7 @@ void btDeformableContactProjection::setConstraints()
|
||||
{
|
||||
if (psb->m_nodes[j].m_im == 0)
|
||||
{
|
||||
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
|
||||
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
|
||||
m_staticConstraints[i].push_back(static_constraint);
|
||||
}
|
||||
}
|
||||
@ -139,7 +139,7 @@ void btDeformableContactProjection::setConstraints()
|
||||
continue;
|
||||
}
|
||||
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
|
||||
btDeformableNodeAnchorConstraint constraint(anchor);
|
||||
btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
|
||||
m_nodeAnchorConstraints[i].push_back(constraint);
|
||||
}
|
||||
|
||||
@ -152,7 +152,7 @@ void btDeformableContactProjection::setConstraints()
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btDeformableNodeRigidContactConstraint constraint(contact);
|
||||
btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
|
||||
btVector3 va = constraint.getVa();
|
||||
btVector3 vb = constraint.getVb();
|
||||
const btVector3 vr = vb - va;
|
||||
@ -173,7 +173,7 @@ void btDeformableContactProjection::setConstraints()
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btDeformableFaceRigidContactConstraint constraint(contact);
|
||||
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal);
|
||||
btVector3 va = constraint.getVa();
|
||||
btVector3 vb = constraint.getVb();
|
||||
const btVector3 vr = vb - va;
|
||||
@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints()
|
||||
{
|
||||
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
|
||||
|
||||
btDeformableFaceNodeContactConstraint constraint(contact);
|
||||
btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal);
|
||||
btVector3 va = constraint.getVa();
|
||||
btVector3 vb = constraint.getVb();
|
||||
const btVector3 vr = vb - va;
|
||||
|
||||
@ -72,13 +72,13 @@ public:
|
||||
virtual void applyDynamicFriction(TVStack& f);
|
||||
|
||||
// update and solve the constraints
|
||||
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
|
||||
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// solve the position error using split impulse
|
||||
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
|
||||
virtual void setConstraints();
|
||||
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Set up projections for each vertex by adding the projection direction to
|
||||
virtual void setProjection();
|
||||
|
||||
@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
|
||||
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
// solver body velocity -> rigid body velocity
|
||||
solverBodyWriteBack(infoGlobal);
|
||||
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
|
||||
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
|
||||
// update rigid body velocity in rigid/deformable contact
|
||||
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
|
||||
// solver body velocity <- rigid body velocity
|
||||
@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
{
|
||||
m_deformableSolver->splitImpulseSetup(infoGlobal);
|
||||
// m_deformableSolver->splitImpulseSetup(infoGlobal);
|
||||
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
|
||||
{
|
||||
btScalar leastSquaresResidual = 0.f;
|
||||
@ -127,8 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
|
||||
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
// solve the position correction between deformable and rigid/multibody
|
||||
btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
|
||||
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
|
||||
// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
|
||||
{
|
||||
|
||||
@ -285,7 +285,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
|
||||
void btDeformableMultiBodyDynamicsWorld::setupConstraints()
|
||||
{
|
||||
// set up constraints between multibody and deformable bodies
|
||||
m_deformableBodySolver->setConstraints();
|
||||
m_deformableBodySolver->setConstraints(m_solverInfo);
|
||||
|
||||
// set up constraints among multibodies
|
||||
{
|
||||
|
||||
@ -53,6 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
|
||||
n.m_material = pm;
|
||||
}
|
||||
updateBounds();
|
||||
setCollisionQuadrature(3);
|
||||
}
|
||||
|
||||
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
|
||||
@ -2403,10 +2404,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
|
||||
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
|
||||
// btTransform wtr = (predict) ?
|
||||
// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
|
||||
// : colObjWrap->getWorldTransform();
|
||||
const btTransform& wtr = colObjWrap->getWorldTransform();
|
||||
btTransform wtr = (predict) ?
|
||||
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
|
||||
: colObjWrap->getWorldTransform();
|
||||
btScalar dst =
|
||||
m_worldInfo->m_sparsesdf.Evaluate(
|
||||
wtr.invXform(x),
|
||||
@ -2457,10 +2457,9 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
btTransform wtr = (predict) ?
|
||||
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
|
||||
: colObjWrap->getWorldTransform();
|
||||
// const btTransform& wtr = colObjWrap->getWorldTransform();
|
||||
btScalar dst;
|
||||
|
||||
//#define USE_QUADRATURE 1
|
||||
#define USE_QUADRATURE 1
|
||||
//#define CACHE_PREV_COLLISION
|
||||
|
||||
// use the contact position of the previous collision
|
||||
@ -2476,6 +2475,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
nrm,
|
||||
margin);
|
||||
nrm = wtr.getBasis() * nrm;
|
||||
cti.m_colObj = colObjWrap->getCollisionObject();
|
||||
// use cached contact point
|
||||
}
|
||||
else
|
||||
@ -2492,10 +2492,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
contact_point = results.witnesses[0];
|
||||
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
|
||||
nrm = results.normal;
|
||||
cti.m_colObj = colObjWrap->getCollisionObject();
|
||||
for (int i = 0; i < 3; ++i)
|
||||
f.m_pcontact[i] = bary[i];
|
||||
}
|
||||
|
||||
return (dst < 0);
|
||||
#endif
|
||||
|
||||
// use collision quadrature point
|
||||
@ -2505,7 +2506,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
btVector3 local_nrm;
|
||||
for (int q = 0; q < m_quads.size(); ++q)
|
||||
{
|
||||
btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
|
||||
btVector3 p;
|
||||
if (predict)
|
||||
p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
|
||||
else
|
||||
p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
|
||||
btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
|
||||
wtr.invXform(p),
|
||||
shp,
|
||||
@ -2513,12 +2518,21 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
margin);
|
||||
if (local_dst < dst)
|
||||
{
|
||||
if (local_dst < 0 && predict)
|
||||
return true;
|
||||
dst = local_dst;
|
||||
contact_point = p;
|
||||
bary = m_quads[q];
|
||||
nrm = wtr.getBasis() * local_nrm;
|
||||
nrm = local_nrm;
|
||||
}
|
||||
if (!predict)
|
||||
{
|
||||
cti.m_colObj = colObjWrap->getCollisionObject();
|
||||
cti.m_normal = wtr.getBasis() * nrm;
|
||||
cti.m_offset = dst;
|
||||
}
|
||||
}
|
||||
return (dst < 0);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -2530,6 +2544,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
triangle_transform.setOrigin(f.m_n[0]->m_x);
|
||||
btTriangleShape triangle(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);
|
||||
btVector3 guess(0,0,0);
|
||||
if (predict)
|
||||
{
|
||||
triangle_transform.setOrigin(f.m_n[0]->m_q);
|
||||
triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
|
||||
}
|
||||
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
|
||||
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
|
||||
dst = results.distance - margin;
|
||||
@ -2547,9 +2566,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
|
||||
cti.m_offset = dst;
|
||||
}
|
||||
|
||||
if (dst < 0)
|
||||
return true;
|
||||
return (false);
|
||||
return (dst < 0);
|
||||
}
|
||||
|
||||
//
|
||||
@ -3681,8 +3698,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
|
||||
docollideFace.psb = this;
|
||||
docollideFace.m_colObj1Wrap = pcoWrap;
|
||||
docollideFace.m_rigidBody = prb1;
|
||||
docollideFace.dynmargin = basemargin + timemargin;
|
||||
docollideFace.stamargin = basemargin;
|
||||
docollideFace.dynmargin = 0.05*(basemargin + timemargin);
|
||||
docollideFace.stamargin = 0.05*basemargin;
|
||||
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1070,8 +1070,8 @@ struct btSoftColliders
|
||||
|
||||
if (!n.m_battach)
|
||||
{
|
||||
// check for collision at x_{n+1}^* as well at x_n
|
||||
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
|
||||
// check for collision at x_{n+1}^*
|
||||
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
|
||||
{
|
||||
const btScalar ima = n.m_im;
|
||||
// todo: collision between multibody and fixed deformable node will be missed.
|
||||
@ -1159,7 +1159,6 @@ struct btSoftColliders
|
||||
btSoftBody::Node* n0 = f.m_n[0];
|
||||
btSoftBody::Node* n1 = f.m_n[1];
|
||||
btSoftBody::Node* n2 = f.m_n[2];
|
||||
|
||||
const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
|
||||
btSoftBody::DeformableFaceRigidContact c;
|
||||
btVector3 contact_point;
|
||||
@ -1181,6 +1180,8 @@ struct btSoftColliders
|
||||
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
|
||||
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
|
||||
c.m_face = &f;
|
||||
// friction is handled by the nodes to prevent sticking
|
||||
// const btScalar fc = 0;
|
||||
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
|
||||
|
||||
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
|
||||
|
||||
Loading…
Reference in New Issue
Block a user