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

Revert "Reduced Deformable Model"

Browse Source
This commit is contained in:
erwincoumans 2022-03-07 15:38:32 -08:00 committed by GitHub
parent a8e1ce8273
commit 3df6e1a721
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
77 changed files with 114 additions and 30447 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@ -36,7 +36,3 @@ CTestTestFile.cmake
# vim temp files
*.swp
.vscode/
.idea/
cmake-build-debug/

BIN
data/reduced_beam/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_beam/M_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_beam/M_r_diag_mat.bin
View File

Binary file not shown.

83
data/reduced_beam/beam_mesh.vtk
View File

@ -1,83 +0,0 @@
#vtk DataFile Version 2.0
I don't think this matters
ASCII
DATASET UNSTRUCTURED_GRID
POINTS 28 double
-0.5000000000000000 -0.2500000000000000 -2.0000000000000000
-0.5000000000000000 0.2500000000000000 -2.0000000000000000
0.5000000000000000 -0.2500000000000000 -2.0000000000000000
0.5000000000000000 0.2500000000000000 -2.0000000000000000
-0.5000000000000000 -0.2500000000000000 2.0000000000000000
-0.5000000000000000 0.2500000000000000 2.0000000000000000
0.5000000000000000 -0.2500000000000000 2.0000000000000000
0.5000000000000000 0.2500000000000000 2.0000000000000000
-0.5000000000000000 -0.2500000000000000 1.0000000000000000
-0.5000000000000000 -0.2500000000000000 0.0000000000000000
-0.5000000000000000 -0.2500000000000000 -1.0000000000000000
0.5000000000000000 -0.2500000000000000 -1.0000000000000000
0.5000000000000000 -0.2500000000000000 0.0000000000000000
0.5000000000000000 -0.2500000000000000 1.0000000000000000
-0.5000000000000000 0.2500000000000000 1.0000000000000000
-0.5000000000000000 0.2500000000000000 0.0000000000000000
-0.5000000000000000 0.2500000000000000 -1.0000000000000000
0.5000000000000000 0.2500000000000000 1.0000000000000000
0.5000000000000000 0.2500000000000000 0.0000000000000000
0.5000000000000000 0.2500000000000000 -1.0000000000000000
0.0000000000000000 -0.2500000000000000 -1.5000000000000000
0.0000000000000000 -0.2500000000000000 -0.5000000000000000
0.0000000000000000 -0.2500000000000000 0.5000000000000000
0.0000000000000000 -0.2500000000000000 1.5000000000000000
0.0000000000000000 0.2500000000000000 1.5000000000000000
0.0000000000000000 0.2500000000000000 0.5000000000000000
0.0000000000000000 0.2500000000000000 -0.5000000000000000
0.0000000000000000 0.2500000000000000 -1.5000000000000000
CELLS 48 192
4 22 12 17 13
4 20 26 21 11
4 23 8 5 24
4 11 27 26 19
4 27 11 26 20
4 17 22 24 25
4 7 23 13 6
4 22 17 24 13
4 9 25 21 15
4 24 22 8 14
4 24 8 22 23
4 25 9 21 22
4 23 7 13 24
4 15 10 16 26
4 14 25 9 15
4 25 14 9 22
4 24 23 22 13
4 15 10 21 9
4 15 21 10 26
4 24 25 22 14
4 2 11 27 20
4 7 5 23 6
4 11 21 12 26
4 20 1 0 16
4 18 25 12 17
4 18 12 25 26
4 5 7 23 24
4 26 21 25 15
4 20 16 27 1
4 21 25 22 12
4 20 16 10 26
4 20 27 16 26
4 20 10 16 0
4 11 12 19 26
4 14 22 8 9
4 8 23 5 4
4 24 14 8 5
4 11 2 27 3
4 2 27 1 20
4 12 18 19 26
4 27 2 1 3
4 24 13 17 7
4 20 2 0 1
4 5 23 6 4
4 20 10 21 26
4 12 22 17 25
4 3 27 11 19
4 21 26 25 12
CELL_TYPES 10

83
data/reduced_beam/beam_mesh_origin.vtk
View File

@ -1,83 +0,0 @@
#vtk DataFile Version 2.0
I don't think this matters
ASCII
DATASET UNSTRUCTURED_GRID
POINTS 28 double
0.0000000000000000 0.0000000000000000 0.0000000000000000
0.0000000000000000 0.5000000000000000 0.0000000000000000
1.0000000000000000 0.0000000000000000 0.0000000000000000
1.0000000000000000 0.5000000000000000 0.0000000000000000
0.0000000000000000 0.0000000000000000 4.0000000000000000
0.0000000000000000 0.5000000000000000 4.0000000000000000
1.0000000000000000 0.0000000000000000 4.0000000000000000
1.0000000000000000 0.5000000000000000 4.0000000000000000
0.0000000000000000 0.0000000000000000 3.0000000000000000
0.0000000000000000 0.0000000000000000 2.0000000000000000
0.0000000000000000 0.0000000000000000 1.0000000000000000
1.0000000000000000 0.0000000000000000 1.0000000000000000
1.0000000000000000 0.0000000000000000 2.0000000000000000
1.0000000000000000 0.0000000000000000 3.0000000000000000
0.0000000000000000 0.5000000000000000 3.0000000000000000
0.0000000000000000 0.5000000000000000 2.0000000000000000
0.0000000000000000 0.5000000000000000 1.0000000000000000
1.0000000000000000 0.5000000000000000 3.0000000000000000
1.0000000000000000 0.5000000000000000 2.0000000000000000
1.0000000000000000 0.5000000000000000 1.0000000000000000
0.5000000000000000 0.0000000000000000 0.5000000000000000
0.5000000000000000 0.0000000000000000 1.5000000000000000
0.5000000000000000 0.0000000000000000 2.5000000000000000
0.5000000000000000 0.0000000000000000 3.5000000000000000
0.5000000000000000 0.5000000000000000 3.5000000000000000
0.5000000000000000 0.5000000000000000 2.5000000000000000
0.5000000000000000 0.5000000000000000 1.5000000000000000
0.5000000000000000 0.5000000000000000 0.5000000000000000
CELLS 48 192
4 22 12 17 13
4 20 26 21 11
4 23 8 5 24
4 11 27 26 19
4 27 11 26 20
4 17 22 24 25
4 7 23 13 6
4 22 17 24 13
4 9 25 21 15
4 24 22 8 14
4 24 8 22 23
4 25 9 21 22
4 23 7 13 24
4 15 10 16 26
4 14 25 9 15
4 25 14 9 22
4 24 23 22 13
4 15 10 21 9
4 15 21 10 26
4 24 25 22 14
4 2 11 27 20
4 7 5 23 6
4 11 21 12 26
4 20 1 0 16
4 18 25 12 17
4 18 12 25 26
4 5 7 23 24
4 26 21 25 15
4 20 16 27 1
4 21 25 22 12
4 20 16 10 26
4 20 27 16 26
4 20 10 16 0
4 11 12 19 26
4 14 22 8 9
4 8 23 5 4
4 24 14 8 5
4 11 2 27 3
4 2 27 1 20
4 12 18 19 26
4 27 2 1 3
4 24 13 17 7
4 20 2 0 1
4 5 23 6 4
4 20 10 21 26
4 12 22 17 25
4 3 27 11 19
4 21 26 25 12
CELL_TYPES 10

BIN
data/reduced_beam/eigenvalues.bin
View File

Binary file not shown.

BIN
data/reduced_beam/modes.bin
View File

Binary file not shown.

14
data/reduced_beam/reduced_beam.urdf
View File

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<robot name="reduced_cube">
<reduced_deformable name="reduced_beam">
<num_modes value="20"/>
<mass value="1"/>
<stiffness_scale value="100"/>
<collision_margin value="0.01"/>
<erp value= "0.2"/>
<cfm value= "0.2"/>
<friction value= "0.5"/>
<damping_coefficient value="0.0"/>
<visual filename="beam_mesh_origin.vtk"/>
</reduced_deformable>
</robot>

BIN
data/reduced_cube/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_cube/M_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_cube/M_r_diag_mat.bin
View File

Binary file not shown.

1134
data/reduced_cube/cube_mesh.vtk
View File

File diff suppressed because it is too large Load Diff

14
data/reduced_cube/deform_cube.urdf
View File

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<robot name="cube">
<deformable name="cube">
<inertial>
<mass value="10" />
<inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<collision_margin value="0.006"/>
<repulsion_stiffness value="800.0"/>
<friction value= "0.5"/>
<neohookean mu= "180" lambda= "600" damping= "0.01" />
<visual filename="cube_mesh.vtk"/>
</deformable>
</robot>

BIN
data/reduced_cube/eigenvalues.bin
View File

Binary file not shown.

BIN
data/reduced_cube/modes.bin
View File

Binary file not shown.

14
data/reduced_cube/reduced_cube.urdf
View File

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<robot name="reduced_cube">
<reduced_deformable name="reduced_cube">
<num_modes value="20"/>
<mass value="10"/>
<stiffness_scale value="100"/>
<collision_margin value="0.01"/>
<erp value= "0.2"/>
<cfm value= "0.2"/>
<friction value= "0.5"/>
<damping_coefficient value="0.0"/>
<visual filename="cube_mesh.vtk"/>
</reduced_deformable>
</robot>

BIN
data/reduced_torus/K_r_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_torus/M_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_torus/M_r_diag_mat.bin
View File

Binary file not shown.

BIN
data/reduced_torus/eigenvalues.bin
View File

Binary file not shown.

BIN
data/reduced_torus/modes.bin
View File

Binary file not shown.

15
data/reduced_torus/reduced_torus.urdf
View File

@ -1,15 +0,0 @@
<?xml version="1.0"?>
<robot name="reduced_cube">
<reduced_deformable name="reduced_cube">
<num_modes value="20"/>
<mass value="10"/>
<stiffness_scale value="10"/>
<collision_margin value="0.01"/>
<erp value= "0.2"/>
<cfm value= "0.2"/>
<friction value= "0.5"/>
<damping_coefficient value="0.00001"/>
<visual filename="torus_textured.obj"/>
<collision filename="torus_mesh.vtk"/>
</reduced_deformable>
</robot>

9917
data/reduced_torus/torus_mesh.vtk
View File

File diff suppressed because it is too large Load Diff

9917
data/reduced_torus/torus_mesh_visual.vtk
View File

File diff suppressed because it is too large Load Diff

2270
data/reduced_torus/torus_textured.obj
View File

File diff suppressed because it is too large Load Diff

18
examples/ExampleBrowser/CMakeLists.txt
View File

@ -390,24 +390,6 @@ SET(BulletExampleBrowser_SRCS
../MultiBody/MultiDofDemo.h
../RigidBody/RigidBodySoftContact.cpp
../RigidBody/KinematicRigidBodyExample.cpp
../ReducedDeformableDemo/ConservationTest.cpp
../ReducedDeformableDemo/ConservationTest.h
../ReducedDeformableDemo/Springboard.cpp
../ReducedDeformableDemo/Springboard.h
../ReducedDeformableDemo/ModeVisualizer.cpp
../ReducedDeformableDemo/ModeVisualizer.h
../ReducedDeformableDemo/FreeFall.cpp
../ReducedDeformableDemo/FreeFall.h
../ReducedDeformableDemo/FrictionSlope.cpp
../ReducedDeformableDemo/FrictionSlope.h
../ReducedDeformableDemo/ReducedCollide.cpp
../ReducedDeformableDemo/ReducedCollide.h
../ReducedDeformableDemo/ReducedGrasp.cpp
../ReducedDeformableDemo/ReducedGrasp.h
../ReducedDeformableDemo/ReducedBenchmark.cpp
../ReducedDeformableDemo/ReducedBenchmark.h
../ReducedDeformableDemo/ReducedMotorGrasp.cpp
../ReducedDeformableDemo/ReducedMotorGrasp.h
../Constraints/TestHingeTorque.cpp
../Constraints/TestHingeTorque.h
../Constraints/ConstraintDemo.cpp

21
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -73,15 +73,6 @@
#include "../RoboticsLearning/R2D2GraspExample.h"
#include "../RoboticsLearning/KukaGraspExample.h"
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../ReducedDeformableDemo/ConservationTest.h"
#include "../ReducedDeformableDemo/ModeVisualizer.h"
#include "../ReducedDeformableDemo/Springboard.h"
#include "../ReducedDeformableDemo/FreeFall.h"
#include "../ReducedDeformableDemo/FrictionSlope.h"
#include "../ReducedDeformableDemo/ReducedCollide.h"
#include "../ReducedDeformableDemo/ReducedGrasp.h"
#include "../ReducedDeformableDemo/ReducedMotorGrasp.h"
#include "../ReducedDeformableDemo/ReducedBenchmark.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef B3_ENABLE_TINY_AUDIO
@ -225,18 +216,6 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
// ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
ExampleEntry(0, "Reduced Deformabe Body"),
ExampleEntry(1, "Mode Visualizer", "Visualizer the modes for reduced deformable objects", ReducedModeVisualizerCreateFunc),
ExampleEntry(1, "Reduced Conservation Test", "Momentum conservation test for the reduced deformable objects", ReducedConservationTestCreateFunc),
ExampleEntry(1, "Reduced Springboard", "Moving rigid object colliding with a fixed reduced deformable objects", ReducedSpringboardCreateFunc),
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 Motor Grasp", "Grasp a reduced deformable block with motor", ReducedMotorGraspCreateFunc),
ExampleEntry(1, "Reduced Friction Slope", "Grasp a reduced deformable block", FrictionSlopeCreateFunc),
ExampleEntry(1, "Reduced Benchmark", "Reduced deformable performance benchmark example", ReducedBenchmarkCreateFunc),
// ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),
#ifdef INCLUDE_CLOTH_DEMOS
ExampleEntry(0, "Soft Body"),

5
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -1541,8 +1541,3 @@ const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const
{
return m_data->m_urdfParser.getDeformable();
}
const struct UrdfReducedDeformable& BulletURDFImporter::getReducedDeformableModel() const
{
return m_data->m_urdfParser.getReducedDeformable();
}

1
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@ -93,7 +93,6 @@ public:
virtual void setEnableTinyRenderer(bool enable);
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
const struct UrdfDeformable& getDeformableModel() const;
const struct UrdfReducedDeformable& getReducedDeformableModel() const;
};
#endif //BULLET_URDF_IMPORTER_H

174
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -1282,171 +1282,6 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
return true;
}
bool UrdfParser::parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
{
UrdfReducedDeformable& reduced_deformable = model.m_reducedDeformable;
const char* name = config->Attribute("name");
if (!name)
{
logger->reportError("Reduced deformable with no name");
return false;
}
reduced_deformable.m_name = name;
{
XMLElement* numModes_xml = config->FirstChildElement("num_modes");
if (numModes_xml)
{
if (!numModes_xml->Attribute("value"))
{
logger->reportError("numModes_xml element must have value attribute");
return false;
}
reduced_deformable.m_numModes = urdfLexicalCast<double>(numModes_xml->Attribute("value"));
}
}
{
XMLElement* mass_xml = config->FirstChildElement("mass");
if (mass_xml)
{
if (!mass_xml->Attribute("value"))
{
logger->reportError("mass_xml element must have value attribute");
return false;
}
reduced_deformable.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
}
}
{
XMLElement* stiffnessScale_xml = config->FirstChildElement("stiffness_scale");
if (stiffnessScale_xml)
{
if (!stiffnessScale_xml->Attribute("value"))
{
logger->reportError("stiffnessScale_xml element must have value attribute");
return false;
}
reduced_deformable.m_stiffnessScale = urdfLexicalCast<double>(stiffnessScale_xml->Attribute("value"));
}
}
{
XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin");
if (collisionMargin_xml)
{
if (!collisionMargin_xml->Attribute("value"))
{
logger->reportError("collision_margin element must have value attribute");
return false;
}
reduced_deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
}
}
{
XMLElement* erp_xml = config->FirstChildElement("erp");
if (erp_xml)
{
if (!erp_xml->Attribute("value"))
{
logger->reportError("friction element must have value attribute");
return false;
}
reduced_deformable.m_erp = urdfLexicalCast<double>(erp_xml->Attribute("value"));
}
}
{
XMLElement* cfm_xml = config->FirstChildElement("cfm");
if (cfm_xml)
{
if (!cfm_xml->Attribute("value"))
{
logger->reportError("cfm element must have value attribute");
return false;
}
reduced_deformable.m_cfm = urdfLexicalCast<double>(cfm_xml->Attribute("value"));
}
}
{
XMLElement* damping_xml = config->FirstChildElement("damping_coefficient");
if (damping_xml)
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("damping_coefficient element must have value attribute");
return false;
}
reduced_deformable.m_damping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
}
}
{
XMLElement* friction_xml = config->FirstChildElement("friction");
if (friction_xml)
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("friction element must have value attribute");
return false;
}
reduced_deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
XMLElement* vis_xml = config->FirstChildElement("visual");
if (!vis_xml)
{
logger->reportError("expected an visual element");
return false;
}
if (!vis_xml->Attribute("filename"))
{
logger->reportError("expected a filename for visual geometry");
return false;
}
std::string fn = vis_xml->Attribute("filename");
reduced_deformable.m_visualFileName = fn;
int out_type(0);
bool success = UrdfFindMeshFile(m_fileIO,
model.m_sourceFile, fn, sourceFileLocation(vis_xml),
&reduced_deformable.m_visualFileName, &out_type);
if (!success)
{
// warning already printed
return false;
}
// collision mesh is optional
XMLElement* col_xml = config->FirstChildElement("collision");
if (col_xml)
{
if (!col_xml->Attribute("filename"))
{
logger->reportError("expected a filename for collision geoemtry");
return false;
}
fn = col_xml->Attribute("filename");
success = UrdfFindMeshFile(m_fileIO,
model.m_sourceFile, fn, sourceFileLocation(col_xml),
&reduced_deformable.m_simFileName, &out_type);
if (!success)
{
// warning already printed
return false;
}
}
ParseUserData(config, reduced_deformable.m_userData, logger);
return true;
}
bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
@ -2291,16 +2126,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
// logger->printMessage(msg);
XMLElement* reduced_deformable_xml = robot_xml->FirstChildElement("reduced_deformable");
if (reduced_deformable_xml) {
return parseReducedDeformable(m_urdf2Model, reduced_deformable_xml, logger);
}
XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
if (deformable_xml) {
if(deformable_xml)
return parseDeformable(m_urdf2Model, deformable_xml, logger);
}
for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{

39
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -247,37 +247,6 @@ struct UrdfDeformable
}
};
struct UrdfReducedDeformable
{
std::string m_name;
int m_numModes;
double m_mass;
double m_stiffnessScale;
double m_erp;
double m_cfm;
double m_friction;
double m_collisionMargin;
double m_damping;
std::string m_visualFileName;
std::string m_simFileName;
btHashMap<btHashString, std::string> m_userData;
UrdfReducedDeformable()
: m_numModes(1),
m_mass(1),
m_stiffnessScale(100),
m_erp(0.2), // generally, 0.2 is a good value for erp and cfm
m_cfm(0.2),
m_friction(0),
m_collisionMargin(0.02),
m_damping(0),
m_visualFileName(""),
m_simFileName("")
{}
};
struct UrdfModel
{
std::string m_name;
@ -287,7 +256,6 @@ struct UrdfModel
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
UrdfDeformable m_deformable;
UrdfReducedDeformable m_reducedDeformable;
// Maps user data keys to user data values.
btHashMap<btHashString, std::string> m_userData;
@ -365,7 +333,7 @@ protected:
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser(struct CommonFileIOInterface* fileIO);
@ -445,11 +413,6 @@ public:
return m_urdf2Model.m_deformable;
}
const UrdfReducedDeformable& getReducedDeformable() const
{
return m_urdf2Model.m_reducedDeformable;
}
bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);

2
examples/MultiBody/MultiDofDemo.cpp
View File

@ -165,7 +165,7 @@ void MultiDofDemo::initPhysics()
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
mbC->setCanSleep(canSleep);

316
examples/ReducedDeformableDemo/ConservationTest.cpp
View File

@ -1,316 +0,0 @@
/*
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 "ConservationTest.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <stdio.h> //printf debugging
#include <random>
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0;
static int start_mode = 6;
static int num_modes = 20;
class ConservationTest : public CommonDeformableBodyBase
{
btScalar sim_time;
bool first_step;
// get deformed shape
void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar scale = 1)
{
// for (int i = 0; i < rsb->m_nodes.size(); ++i)
// for (int k = 0; k < 3; ++k)
// rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale;
// rsb->m_reducedDofs[mode_n] = scale;
// rsb->m_reducedDofsBuffer[mode_n] = scale;
srand(1);
for (int r = 0; r < rsb->m_nReduced; r++)
{
rsb->m_reducedDofs[r] = (btScalar(rand()) / btScalar(RAND_MAX) - 0.5);
rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r];
}
rsb->mapToFullPosition(rsb->getRigidTransform());
// std::cout << "-----------\n";
// std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
// std::cout << "-----------\n";
}
public:
ConservationTest(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
first_step = true;
}
virtual ~ConservationTest()
{
}
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 = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void checkMomentum(btReducedDeformableBody* rsb)
{
btVector3 x_com(0, 0, 0);
btVector3 total_linear(0, 0, 0);
btVector3 total_angular(0, 0, 0);
{
std::ofstream myfile("center_of_mass.txt", std::ios_base::app);
for (int i = 0; i < rsb->m_nFull; ++i)
{
x_com += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_x;
}
x_com /= rsb->getTotalMass();
myfile << sim_time << "\t" << x_com[0] << "\t" << x_com[1] << "\t" << x_com[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("linear_momentum.txt", std::ios_base::app);
for (int i = 0; i < rsb->m_nFull; ++i)
{
total_linear += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_v;
}
myfile << sim_time << "\t" << total_linear[0] << "\t" << total_linear[1] << "\t" << total_linear[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("angular_momentum.txt", std::ios_base::app);
// btVector3 ri(0, 0, 0);
// for (int i = 0; i < rsb->m_nFull; ++i)
// {
// ri = rsb->m_nodes[i].m_x - x_com;
// total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v);
// }
total_angular = rsb->computeTotalAngularMomentum();
myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n";
myfile.close();
}
{
btVector3 angular_rigid(0, 0, 0);
std::ofstream myfile("angular_momentum_rigid.txt", std::ios_base::app);
btVector3 ri(0, 0, 0);
for (int i = 0; i < rsb->m_nFull; ++i)
{
ri = rsb->m_nodes[i].m_x - x_com;
btMatrix3x3 ri_star = Cross(ri);
angular_rigid += rsb->m_nodalMass[i] * (ri_star.transpose() * ri_star * rsb->getAngularVelocity());
}
myfile << sim_time << "\t" << angular_rigid[0] << "\t" << angular_rigid[1] << "\t" << angular_rigid[2] << "\n";
myfile.close();
}
{
std::ofstream myfile("reduced_velocity.txt", std::ios_base::app);
myfile << sim_time << "\t" << rsb->m_reducedVelocity[0] << "\t" << rsb->m_reducedDofs[0] << "\n";
myfile.close();
}
}
void stepSimulation(float deltaTime)
{
// add initial deformation
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
if (first_step /* && !rsb->m_bUpdateRtCst*/)
{
getDeformedShape(rsb, 0, 1);
first_step = false;
}
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
sim_time += internalTimeStep;
checkMomentum(rsb);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
btVector3 origin = rsb->getRigidTransform().getOrigin();
btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin;
btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin;
btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin;
deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0));
deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0));
deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void ConservationTest::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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, 0, 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_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transformTo(init_transform);
rsb->setStiffnessScale(100);
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);
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ConservationTest::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* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options)
{
return new ConservationTest(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ConservationTest.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_CONSERVATION_TEST_H
#define _REDUCED_CONSERVATION_TEST_H
class CommonExampleInterface* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_CONSERVATION_TEST_H

276
examples/ReducedDeformableDemo/FreeFall.cpp
View File

@ -1,276 +0,0 @@
/*
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 "FreeFall.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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.0001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 40;
class FreeFall : public CommonDeformableBodyBase
{
public:
FreeFall(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~FreeFall()
{
}
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 = 8;
float pitch = -10;
float yaw = 90;
float targetPos[3] = {0, 2, 0};
// float dist = 10;
// float pitch = -30;
// float yaw = 125;
// float targetPos[3] = {0, 2, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack(const btVector3& origin)
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 12, 0));
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(origin);
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
rb1->setLinearVelocity(btVector3(0, 0, 0));
}
void createReducedDeformableObject(const btVector3& origin, const btQuaternion& rotation)
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
// rsb->scale(btVector3(1, 1, 0.5));
rsb->setTotalMass(10);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
rsb->transformTo(init_transform);
rsb->setStiffnessScale(25);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
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);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
// int flag = 0;
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(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));
// }
// 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));
// }
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void FreeFall::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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(reducedSoftBodySolver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
btVector3 gravity = btVector3(0, -9.8, 0);
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
// 2 reduced deformable cubes
createReducedDeformableObject(btVector3(0, 4, -2), btQuaternion(0, 0, 0));
createReducedDeformableObject(btVector3(0, 4, 2), btQuaternion(0, 0, 0));
// 2 rigid cubes
Ctor_RbUpStack(btVector3(0, 10, -2));
Ctor_RbUpStack(btVector3(0, 10, 2));
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// 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, -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);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
}
void FreeFall::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* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options)
{
return new FreeFall(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/FreeFall.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_FREE_FALL_H
#define _REDUCED_FREE_FALL_H
class CommonExampleInterface* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_FREE_FALL_H

288
examples/ReducedDeformableDemo/FrictionSlope.cpp
View File

@ -1,288 +0,0 @@
/*
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/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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.001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 20;
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 = 1;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
// 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++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(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));
// }
// 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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
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 file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(50);
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(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
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
View File

@ -1,19 +0,0 @@
/*
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

227
examples/ReducedDeformableDemo/ModeVisualizer.cpp
View File

@ -1,227 +0,0 @@
/*
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 "ModeVisualizer.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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"
static int num_modes = 20;
static btScalar visualize_mode = 0;
static btScalar frequency_scale = 1;
class ModeVisualizer : public CommonDeformableBodyBase
{
btScalar sim_time;
// get deformed shape
void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar time_term = 1)
{
for (int i = 0; i < rsb->m_nodes.size(); ++i)
for (int k = 0; k < 3; ++k)
rsb->m_nodes[i].m_x[k] = rsb->m_x0[i][k] + rsb->m_modes[mode_n][3 * i + k] * time_term;
}
btVector3 computeMassWeightedColumnSum(btReducedDeformableBody* rsb, const int mode_n)
{
btVector3 sum(0, 0, 0);
for (int i = 0; i < rsb->m_nodes.size(); ++i)
{
for (int k = 0; k < 3; ++k)
{
sum[k] += rsb->m_nodalMass[i] * rsb->m_modes[mode_n][3 * i + k];
}
}
return sum;
}
public:
ModeVisualizer(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
}
virtual ~ModeVisualizer()
{
}
void initPhysics();
void exitPhysics();
// disable pick force. non-interactive example.
bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
return false;
}
void resetCamera()
{
float dist = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
sim_time += deltaTime;
int n_mode = floor(visualize_mode);
btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale);
getDeformedShape(rsb, n_mode, scale);
// btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
// std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
// << mass_weighted_column_sum[1] << "\t"
// << mass_weighted_column_sum[2] << "\n";
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* rsb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void ModeVisualizer::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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
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 soft body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 2, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
btSoftBodyHelpers::generateBoundaryFaces(rsb);
}
getDeformableDynamicsWorld()->setImplicit(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Visualize Mode", &visualize_mode);
slider.m_minVal = 0;
slider.m_maxVal = num_modes - 1;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Frequency Reduction", &frequency_scale);
slider.m_minVal = 1;
slider.m_maxVal = 1e3;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ModeVisualizer::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* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options)
{
return new ModeVisualizer(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ModeVisualizer.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_MODE_VISUALIZER_H
#define _REDUCED_MODE_VISUALIZER_H
class CommonExampleInterface* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_MODE_VISUALIZER_H

349
examples/ReducedDeformableDemo/ReducedBenchmark.cpp
View File

@ -1,349 +0,0 @@
/*
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 "ReducedBenchmark.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static btScalar COLLIDING_VELOCITY = 0;
static int num_modes = 20;
static bool run_reduced = true;
class ReducedBenchmark : public CommonDeformableBodyBase
{
btVector3 m_gravity;
public:
ReducedBenchmark(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedBenchmark()
{
}
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 = 6;
// float pitch = -10;
// float yaw = 90;
// float targetPos[3] = {0, 2, 0};
float dist = 10;
float pitch = -30;
float yaw = 125;
float targetPos[3] = {0, 2, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack(const btVector3& origin)
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
// btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
// startTransform.setOrigin(btVector3(0, 12, 0));
// btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
// rb0->setLinearVelocity(btVector3(0, 0, 0));
startTransform.setOrigin(origin);
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
// rb1->setLinearVelocity(btVector3(0, 0, 4));
}
void createDeform(const btVector3& origin, const btQuaternion& rotation)
{
if (run_reduced)
{
std::string file_path("../../../data/reduced_torus/");
std::string vtk_file("torus_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.01);
// rsb->scale(btVector3(1, 1, 0.5));
rsb->setTotalMass(10);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
rsb->transformTo(init_transform);
rsb->setStiffnessScale(5);
rsb->setDamping(damping_alpha, damping_beta);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
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);
std::cout << "Running reduced deformable\n";
}
else // create full deformable cube
{
std::string filepath("../../../data/reduced_torus/");
std::string filename = filepath + "torus_mesh.vtk";
btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(origin);
init_transform.setRotation(rotation);
psb->transform(init_transform);
psb->getCollisionShape()->setMargin(0.015);
psb->setTotalMass(10);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = .5;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
getDeformableDynamicsWorld()->addSoftBody(psb);
btSoftBodyHelpers::generateBoundaryFaces(psb);
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(m_gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btScalar E = 10000;
btScalar nu = 0.3;
btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
btScalar mu = E / (2 * (1 + nu));
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.01);
// neohookean->setPoissonRatio(0.3);
// neohookean->setYoungsModulus(25);
neohookean->setDamping(0.01);
psb->m_cfg.drag = 0.001;
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
std::cout << "Running full deformable\n";
}
// btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
// getDeformableDynamicsWorld()->addSoftBody(rsb);
// rsb->getCollisionShape()->setMargin(0.01);
// // rsb->scale(btVector3(1, 1, 0.5));
// rsb->setTotalMass(10);
// btTransform init_transform;
// init_transform.setIdentity();
// init_transform.setOrigin(origin);
// init_transform.setRotation(rotation);
// rsb->transformTo(init_transform);
// rsb->setStiffnessScale(5);
// rsb->setDamping(damping_alpha, damping_beta);
// // rsb->scale(btVector3(0.5, 0.5, 0.5));
// 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);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
};
void ReducedBenchmark::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();
if (run_reduced)
{
btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
}
else
{
btDeformableBodySolver* solver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
}
// m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
btVector3 gravity = btVector3(0, -10, 0);
m_gravity = gravity;
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// 3x3 torus
createDeform(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
createDeform(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(20), btScalar(2), btScalar(20)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
// 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));
// groundTransform.setOrigin(btVector3(0, -50, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
m_dynamicsWorld->setGravity(gravity);
}
void ReducedBenchmark::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* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedBenchmark(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedBenchmark.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_BENCHMARK_H
#define _REDUCED_BENCHMARK_H
class CommonExampleInterface* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_BENCHMARK_H

522
examples/ReducedDeformableDemo/ReducedCollide.cpp
View File

@ -1,522 +0,0 @@
/*
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 "ReducedCollide.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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 = 4;
static int num_modes = 20;
class ReducedCollide : public CommonDeformableBodyBase
{
public:
ReducedCollide(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedCollide()
{
}
void initPhysics();
void exitPhysics();
btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
// 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 = -10;
float dist = 10;
float pitch = -5;
float yaw = 90;
float targetPos[3] = {0, 0, 0};
// float dist = 5;
// float pitch = -35;
// float yaw = 50;
// float targetPos[3] = {-3, 2.8, -2.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(1, 1, 1));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, COLLIDING_VELOCITY, 0));
// body->setFriction(1);
}
void rigidBar()
{
float mass = 10;
btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,10,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, 0, 0));
// body->setFriction(0);
}
void createGround()
{
// float mass = 55;
float mass = 0;
btCollisionShape* shape = new btBoxShape(btVector3(10, 2, 10));
btVector3 localInertia(0, 0, 0);
if (mass != 0.f)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
// startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body, 1, 1+2);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0, 0, 0));
// body->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++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
{
int index = rsb->m_contactNodesList[p];
deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
}
}
}
};
void ReducedCollide::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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
btVector3 gravity = btVector3(0, 0, 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_dynamicsWorld->setGravity(gravity);
m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;
m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
// rsb->scale(btVector3(0.5, 0.5, 0.5));
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->setTotalMass(15);
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->transformTo(init_transform);
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);
rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
b3Printf("total mass: %e", rsb->getTotalMass());
}
// rigidBar();
// add a few rigid bodies
Ctor_RbUpStack();
// create ground
// createGround();
// create multibody
// {
// bool damping = false;
// bool gyro = true;
// int numLinks = 0;
// bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
// bool multibodyOnly = true;
// bool canSleep = false;
// bool selfCollide = true;
// bool multibodyConstraint = false;
// btVector3 linkHalfExtents(0.05, 0.37, 0.1);
// btVector3 baseHalfExtents(1, 1, 1);
// // btVector3 baseHalfExtents(2.5, 0.5, 2.5);
// // btVector3 baseHalfExtents(0.05, 0.37, 0.1);
// bool g_floatingBase = true;
// // btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0, 4, 0), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
// btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 4.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
// //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
// mbC->setCanSleep(canSleep);
// mbC->setHasSelfCollision(selfCollide);
// mbC->setUseGyroTerm(gyro);
// //
// if (!damping)
// {
// mbC->setLinearDamping(0.f);
// mbC->setAngularDamping(0.f);
// }
// else
// {
// mbC->setLinearDamping(0.1f);
// mbC->setAngularDamping(0.9f);
// }
// //
// //////////////////////////////////////////////
// // if (numLinks > 0)
// // {
// // btScalar q0 = 45.f * SIMD_PI / 180.f;
// // if (!spherical)
// // {
// // mbC->setJointPosMultiDof(0, &q0);
// // }
// // else
// // {
// // btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
// // quat0.normalize();
// // mbC->setJointPosMultiDof(0, quat0);
// // }
// // }
// ///
// addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
// }
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
// {
// 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 ReducedCollide::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;
}
btMultiBody* ReducedCollide::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 10;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// btQuaternion baseOriQuat(btVector3(0, 0, 1), -SIMD_PI / 6.0);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
pMultiBody->setBaseVel(vel);
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void ReducedCollide::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(1);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(1);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedCollide(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedCollide.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_COLLIDE_H
#define _REDUCED_COLLIDE_H
class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_COLLIDE_H

457
examples/ReducedDeformableDemo/ReducedGrasp.cpp
View File

@ -1,457 +0,0 @@
/*
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 "ReducedGrasp.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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.0001;
static int num_modes = 20;
class ReducedGrasp : public CommonDeformableBodyBase
{
public:
ReducedGrasp(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedGrasp()
{
}
void initPhysics();
void exitPhysics();
void resetCamera()
{
float dist = 10;
float pitch = -10;
float yaw = 90;
// float dist = 25;
// float pitch = -30;
// float yaw = 100;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
// float internalTimeStep = 1. / 60.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 1e6;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 0.25));
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1,-4));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape);
}
}
void Ctor_RbUpStack()
{
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,9.5,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
// btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
// {
// btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
// btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
// }
// for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
// {
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.1, btVector3(0, 1, 0));
// }
btSoftBody* psb = static_cast<btSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
}
}
}
static 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 = 0.96;
btScalar pressTime = 1.26;
btScalar liftTime = 2.5;
btScalar shiftTime = 6;
btScalar holdTime = 7;
btScalar dropTime = 10;
// btScalar holdTime = 500;
// btScalar dropTime = 1000;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0,1,0); // inner face has z=2
btVector3 initialTranslationRight = btVector3(0,1,-4); // inner face has z=-2
btVector3 pinchVelocityLeft = btVector3(0,0,-1);
btVector3 pinchVelocityRight = btVector3(0,0,1);
btVector3 liftVelocity = btVector3(0,4,0);
btVector3 shiftVelocity = btVector3(0,0,2);
btVector3 holdVelocity = btVector3(0,0,0);
btVector3 openVelocityLeft = btVector3(0,0,4);
btVector3 openVelocityRight = btVector3(0,0,-4);
if (time < pressTime)
{
velocity = pinchVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * time;
}
// else
// {
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb0->setCenterOfMassTransform(rbTransform);
rb0->setAngularVelocity(btVector3(0,0,0));
rb0->setLinearVelocity(velocity);
btRigidBody* rb1 = rbs[1];
if (time < pressTime)
{
velocity = pinchVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * time;
}
// else
// {
// velocity = btVector3(0, 0, 0);
// translation = initialTranslationRight + pinchVelocityRight * pressTime;
// }
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
}
else
{
velocity = holdVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb1->setCenterOfMassTransform(rbTransform);
rb1->setAngularVelocity(btVector3(0,0,0));
rb1->setLinearVelocity(velocity);
rb0->setFriction(20);
rb1->setFriction(20);
}
void ReducedGrasp::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
// btDeformableBodySolver* solver = new btDeformableBodySolver();
btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(solver);
m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
getDeformableDynamicsWorld()->setSolverCallback(GripperDynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.015);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 1, -2));
// init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(100);
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);
}
// create full deformable cube
{
// std::string filepath("../../../examples/SoftDemo/cube/");
// std::string filename = filepath + "mesh.vtk";
// btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
// // psb->scale(btVector3(2, 2, 2));
// psb->translate(btVector3(0, 1, -2));
// psb->getCollisionShape()->setMargin(0.05);
// psb->setTotalMass(28.6);
// psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
// psb->m_cfg.kCHR = 1; // collision hardness with rigid body
// psb->m_cfg.kDF = .5;
// psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
// getDeformableDynamicsWorld()->addSoftBody(psb);
// btSoftBodyHelpers::generateBoundaryFaces(psb);
// btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
// getDeformableDynamicsWorld()->addForce(psb, gravity_force);
// m_forces.push_back(gravity_force);
// btScalar E = 10000;
// btScalar nu = 0.3;
// btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
// btScalar mu = E / (2 * (1 + nu));
// btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.02);
// // neohookean->setPoissonRatio(0.3);
// // neohookean->setYoungsModulus(25);
// neohookean->setDamping(0.01);
// psb->m_cfg.drag = 0.001;
// getDeformableDynamicsWorld()->addForce(psb, neohookean);
// m_forces.push_back(neohookean);
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// grippers
createGrip();
// rigid block
// Ctor_RbUpStack();
// {
// 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.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -25, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ReducedGrasp::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* ReducedGraspCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedGrasp(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedGrasp.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_GRASP_H
#define _REDUCED_GRASP_H
class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_GRASP_H

558
examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp
View File

@ -1,558 +0,0 @@
/*
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 "ReducedMotorGrasp.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include <stdio.h> //printf debugging
#include "../CommonInterfaces/CommonDeformableBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
///The ReducedMotorGrasp shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
static btScalar sGripperVerticalVelocity = 0.f;
static btScalar sGripperClosingTargetVelocity = 0.f;
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static int num_modes = 20;
static float friction = 1.;
struct TetraCube
{
#include "../SoftDemo/cube.inl"
};
struct TetraBunny
{
#include "../SoftDemo/bunny.inl"
};
static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
class ReducedMotorGrasp : public CommonDeformableBodyBase
{
btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
public:
ReducedMotorGrasp(struct GUIHelperInterface* helper)
:CommonDeformableBodyBase(helper)
{
}
virtual ~ReducedMotorGrasp()
{
}
void initPhysics();
void exitPhysics();
void Ctor_RbUpStack()
{
float mass = 8;
btCollisionShape* shape = new btBoxShape(btVector3(2, 0.25, 0.5));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,0.25,0));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setLinearVelocity(btVector3(0, 0, 0));
rb1->setFriction(0.7);
}
void resetCamera()
{
// float dist = 0.3;
// float pitch = -45;
// float yaw = 100;
// float targetPos[3] = {0, -0.1, 0};
float dist = 0.4;
float pitch = -25;
float yaw = 90;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld,const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating);
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
btMultiBody* createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating);
void stepSimulation(float deltaTime)
{
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
int num_multiBody = getDeformableDynamicsWorld()->getNumMultibodies();
for (int i = 0; i < num_multiBody; ++i)
{
btMultiBody* mb = getDeformableDynamicsWorld()->btMultiBodyDynamicsWorld::getMultiBody(i);
mb->setBaseVel(btVector3(0,sGripperVerticalVelocity, 0));
int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link = 0; link < mb->getNumLinks(); link++)
{
if (supportsJointMotor(mb, link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(10);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
motor->setMaxAppliedImpulse(10);
}
motor->setMaxAppliedImpulse(25);
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
// float internalTimeStep = 1. / 60.f;
// m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
}
void createGrip()
{
int count = 2;
float mass = 2;
btCollisionShape* shape[] = {
new btBoxShape(btVector3(3, 3, 0.5)),
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(10, 0, 0));
startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
createRigidBody(mass, startTransform, shape[i % nshapes]);
}
}
virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
{
return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
}
// for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
// {
// int index = rsb->m_contactNodesList[p];
// deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
// }
}
}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return false;
}
virtual void removePickingConstraint(){}
};
void ReducedMotorGrasp::initPhysics()
{
m_guiHelper->setUpAxis(1);
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
// btVector3 gravity = btVector3(0, 0, 0);
btVector3 gravity = btVector3(0, -9.81, 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_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
// getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
// getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_maxPickingForce = 0.001;
// build a gripper
{
bool damping = true;
bool gyro = false;
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
// btVector3 linkHalfExtents(0.02, 0.018, .003);
// btVector3 baseHalfExtents(0.02, 0.002, .002);
btVector3 linkHalfExtents(0.03, 0.04, 0.006);
btVector3 baseHalfExtents(0.02, 0.015, 0.015);
btVector3 basePosition(0, 0.3, 0);
// btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), baseHalfExtents, linkHalfExtents, false);
btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basePosition, baseHalfExtents, linkHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
for (int i = 0; i < numLinks; i++)
{
int mbLinkIndex = i;
double maxMotorImpulse = 1;
if (supportsJointMotor(mbC, mbLinkIndex))
{
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mbC, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1);
mbC->getLink(mbLinkIndex).m_userPtr = motor;
getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
motor->finalizeMultiDof();
}
}
if (!damping)
{
mbC->setLinearDamping(0.0f);
mbC->setAngularDamping(0.0f);
}
else
{
mbC->setLinearDamping(0.04f);
mbC->setAngularDamping(0.04f);
}
btScalar q0 = 0.f * SIMD_PI / 180.f;
if (numLinks > 0)
mbC->setJointPosMultiDof(0, &q0);
addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
}
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
groundShape->setMargin(0.001);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
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.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(0.5);
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);
}
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_cube/");
std::string vtk_file("cube_mesh.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.001);
rsb->setStiffnessScale(100);
rsb->setDamping(damping_alpha, damping_beta);
rsb->scale(btVector3(0.075, 0.075, 0.075));
rsb->setTotalMass(1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 0.1, 0));
// init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
rsb->transform(init_transform);
// rsb->setRigidVelocity(btVector3(0, 1, 0));
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);
}
// Ctor_RbUpStack();
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
// slider.m_minVal = -.02;
// slider.m_maxVal = .02;
slider.m_minVal = -.2;
slider.m_maxVal = .2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -1;
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ReducedMotorGrasp::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;
}
btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 55;
float linkMass = 55;
int numLinks = 2;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btAlignedObjectArray<btVector3> parentComToCurrentCom;
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, -baseHalfExtents[2] * 2.f));
parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*2.f, 0); //cur body's COM to cur body's PIV offset
btAlignedObjectArray<btVector3> parentComToCurrentPivot;
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));
parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[1] - currentPivotToCurrentCom));//par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot[i], currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void ReducedMotorGrasp::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options)
{
return new ReducedMotorGrasp(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/ReducedMotorGrasp.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_MOTOR_GRASP
#define _REDUCED_MOTOR_GRASP
class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_MOTOR_GRASP

264
examples/ReducedDeformableDemo/Springboard.cpp
View File

@ -1,264 +0,0 @@
/*
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 "Springboard.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.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"
static btScalar damping_alpha = 0.0;
static btScalar damping_beta = 0.0001;
static int num_modes = 20;
class Springboard : public CommonDeformableBodyBase
{
btScalar sim_time;
bool first_step;
public:
Springboard(struct GUIHelperInterface* helper)
: CommonDeformableBodyBase(helper)
{
sim_time = 0;
first_step = true;
}
virtual ~Springboard()
{
}
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 = 10;
float pitch = 0;
float yaw = 90;
float targetPos[3] = {0, 3, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack()
{
float mass = 2;
btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,8,1));
btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
rb1->setActivationState(DISABLE_DEACTIVATION);
}
void stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 60.f;
m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
{
sim_time += internalTimeStep;
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(getDeformableDynamicsWorld()->getSoftBodyArray()[0]);
// std::ofstream myfile("fixed_node.txt", std::ios_base::app);
// myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t"
// << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t"
// << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n";
// myfile.close();
}
}
virtual void renderScene()
{
CommonDeformableBodyBase::renderScene();
btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
{
btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
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));
// std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n";
}
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
// deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
}
}
}
};
void Springboard::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();
btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
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_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
// create volumetric reduced deformable body
{
std::string file_path("../../../data/reduced_beam/");
std::string vtk_file("beam_mesh_origin.vtk");
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
getDeformableDynamicsWorld()->getWorldInfo(),
file_path,
vtk_file,
num_modes,
false);
getDeformableDynamicsWorld()->addSoftBody(rsb);
rsb->getCollisionShape()->setMargin(0.1);
btTransform init_transform;
init_transform.setIdentity();
init_transform.setOrigin(btVector3(0, 4, 0));
// init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
rsb->transform(init_transform);
rsb->setStiffnessScale(200);
rsb->setDamping(damping_alpha, damping_beta);
// set fixed nodes
rsb->setFixedNodes(0);
rsb->setFixedNodes(1);
rsb->setFixedNodes(2);
rsb->setFixedNodes(3);
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);
// rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
// rsb->setRigidVelocity(btVector3(0, 1, 0));
// rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
}
getDeformableDynamicsWorld()->setImplicit(false);
getDeformableDynamicsWorld()->setLineSearch(false);
getDeformableDynamicsWorld()->setUseProjection(false);
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
// add a few rigid bodies
Ctor_RbUpStack();
// create a static rigid box as the ground
{
// btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, 0));
{
btScalar mass(0.);
createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Springboard::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* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options)
{
return new Springboard(options.m_guiHelper);
}

19
examples/ReducedDeformableDemo/Springboard.h
View File

@ -1,19 +0,0 @@
/*
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 _REDUCED_SPRINGBOARD_H
#define _REDUCED_SPRINGBOARD_H
class CommonExampleInterface* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options);
#endif //_REDUCED_SPRINGBOARD_H

481
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -120,10 +120,6 @@
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#endif //SKIP_DEFORMABLE_BODY
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@ -1669,7 +1665,6 @@ struct PhysicsServerCommandProcessorInternalData
btDeformableMousePickingForce* m_mouseForce;
btScalar m_maxPickingForce;
btDeformableBodySolver* m_deformablebodySolver;
btReducedDeformableBodySolver* m_reducedSoftBodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif
@ -2726,26 +2721,16 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
m_data->m_broadphase = bv;
}
#ifndef SKIP_DEFORMABLE_BODY
if (flags & RESET_USE_DEFORMABLE_WORLD)
{
// deformable
#ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
}
else if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD)
{
// reduced deformable
m_data->m_reducedSoftBodySolver = new btReducedDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_reducedSoftBodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_reducedSoftBodySolver);
}
#endif
}
@ -2792,14 +2777,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2; //need to check if there are artifacts with frictionERP
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD)
{
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128;
}
else
{
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
}
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = 0.1;
gDbvtMargin = btScalar(0);
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
@ -3673,11 +3651,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
return false;
}
}
if (!(u2b.getReducedDeformableModel().m_visualFileName.empty()))
{
bool use_self_collision = false;
return processReducedDeformable(u2b.getReducedDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision);
}
bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
if (ok)
{
@ -9439,10 +9412,6 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->setGravityFactor(deformable.m_gravFactor);
psb->setCacheBarycenter(deformable.m_cache_barycenter);
psb->initializeFaceTree();
deformWorld->setImplicit(true);
deformWorld->setLineSearch(false);
deformWorld->setUseProjection(true);
}
#endif //SKIP_DEFORMABLE_BODY
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@ -9704,447 +9673,6 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
return true;
}
bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDeformable& reduced_deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
{
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btReducedDeformableBody* rsb = NULL;
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (fileIO->findResourcePath(reduced_deformable.m_visualFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix = "";
std::string out_found_filename, out_found_sim_filename;
int out_type(0), out_sim_type(0);
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (!reduced_deformable.m_simFileName.empty())
{
bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, reduced_deformable.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
}
else
{
out_sim_type = out_type;
out_found_sim_filename = out_found_filename;
}
if (out_sim_type == UrdfGeometry::FILE_OBJ)
{
printf("Obj file is currently unsupported\n");
return false;
}
else if (out_sim_type == UrdfGeometry::FILE_VTK)
{
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
rsb = btReducedDeformableBodyHelpers::createFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
if (!rsb)
{
printf("Load reduced deformable failed\n");
return false;
}
// load modes, reduced stiffness matrix
rsb->setReducedModes(reduced_deformable.m_numModes, rsb->m_nodes.size());
rsb->setStiffnessScale(reduced_deformable.m_stiffnessScale);
rsb->setDamping(0, reduced_deformable.m_damping); // damping alpha is set to 0 by default
btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, pathPrefix);
// set total mass
rsb->setTotalMass(reduced_deformable.m_mass);
}
#endif
}
b3ImportMeshData meshData;
if (rsb != NULL)
{
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
// load render mesh
if ((out_found_sim_filename != out_found_filename) || ((out_sim_type == UrdfGeometry::FILE_OBJ)))
{
// load render mesh
if (!m_data->m_useAlternativeDeformableIndexing)
{
float rgbaColor[4] = { 1,1,1,1 };
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(
out_found_filename.c_str(), meshData, fileIO))
{
for (int v = 0; v < meshData.m_gfxShape->m_numvertices; v++)
{
btSoftBody::RenderNode n;
n.m_x.setValue(
meshData.m_gfxShape->m_vertices->at(v).xyzw[0],
meshData.m_gfxShape->m_vertices->at(v).xyzw[1],
meshData.m_gfxShape->m_vertices->at(v).xyzw[2]);
n.m_uv1.setValue(meshData.m_gfxShape->m_vertices->at(v).uv[0],
meshData.m_gfxShape->m_vertices->at(v).uv[1],
0.);
n.m_normal.setValue(meshData.m_gfxShape->m_vertices->at(v).normal[0],
meshData.m_gfxShape->m_vertices->at(v).normal[1],
meshData.m_gfxShape->m_vertices->at(v).normal[2]);
rsb->m_renderNodes.push_back(n);
}
for (int f = 0; f < meshData.m_gfxShape->m_numIndices; f += 3)
{
btSoftBody::RenderFace ff;
ff.m_n[0] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 0)];
ff.m_n[1] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 1)];
ff.m_n[2] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 2)];
rsb->m_renderFaces.push_back(ff);
}
}
}
else
{
bt_tinyobj::attrib_t attribute;
std::vector<bt_tinyobj::shape_t> shapes;
std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
for (int s = 0; s < (int)shapes.size(); s++)
{
bt_tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
int vertexCount = attribute.vertices.size() / 3;
for (int v = 0; v < vertexCount; v++)
{
btSoftBody::RenderNode n;
n.m_x = btVector3(attribute.vertices[3 * v], attribute.vertices[3 * v + 1], attribute.vertices[3 * v + 2]);
rsb->m_renderNodes.push_back(n);
}
for (int f = 0; f < faceCount; f += 3)
{
if (f < 0 && f >= int(shape.mesh.indices.size()))
{
continue;
}
bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
btSoftBody::RenderFace ff;
ff.m_n[0] = &rsb->m_renderNodes[v_0.vertex_index];
ff.m_n[1] = &rsb->m_renderNodes[v_1.vertex_index];
ff.m_n[2] = &rsb->m_renderNodes[v_2.vertex_index];
rsb->m_renderFaces.push_back(ff);
}
}
}
if (out_sim_type == UrdfGeometry::FILE_VTK)
{
btSoftBodyHelpers::interpolateBarycentricWeights(rsb);
}
else if (out_sim_type == UrdfGeometry::FILE_OBJ)
{
btSoftBodyHelpers::extrapolateBarycentricWeights(rsb);
}
}
else
{
rsb->m_renderNodes.resize(0);
}
#endif
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
btScalar collision_hardness = 1;
rsb->m_cfg.kKHR = collision_hardness;
rsb->m_cfg.kCHR = collision_hardness;
rsb->m_cfg.kDF = reduced_deformable.m_friction;
btSoftBody::Material* pm = rsb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
// turn on the collision flag for deformable
// collision between deformable and rigid
rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
/// turn on node contact for rigid body
rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
// turn on face contact for multibodies
// rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
// collion between deformable and deformable and self-collision
// rsb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
rsb->setCollisionFlags(0);
rsb->setSelfCollision(useSelfCollision);
rsb->initializeFaceTree();
}
#endif //SKIP_DEFORMABLE_BODY
// #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
// btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
// if (softWorld)
// {
// btSoftBody::Material* pm = rsb->appendMaterial();
// pm->m_kLST = 0.5;
// pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
// rsb->generateBendingConstraints(2, pm);
// rsb->m_cfg.piterations = 20;
// rsb->m_cfg.kDF = 0.5;
// //turn on softbody vs softbody collision
// rsb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
// rsb->randomizeConstraints();
// rsb->setTotalMass(reduced_deformable.m_mass, true);
// }
// #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
rsb->scale(btVector3(scale, scale, scale));
btTransform init_transform;
init_transform.setOrigin(pos);
init_transform.setRotation(orn);
rsb->transform(init_transform);
rsb->getCollisionShape()->setMargin(reduced_deformable.m_collisionMargin);
rsb->getCollisionShape()->setUserPointer(rsb);
#ifndef SKIP_DEFORMABLE_BODY
if (deformWorld)
{
deformWorld->addSoftBody(rsb);
deformWorld->getSolverInfo().m_deformable_erp = reduced_deformable.m_erp;
deformWorld->getSolverInfo().m_deformable_cfm = reduced_deformable.m_cfm;
deformWorld->getSolverInfo().m_friction = reduced_deformable.m_friction;
deformWorld->getSolverInfo().m_splitImpulse = false;
deformWorld->setImplicit(false);
deformWorld->setLineSearch(false);
deformWorld->setUseProjection(false);
}
else
#endif //SKIP_DEFORMABLE_BODY
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->addSoftBody(rsb);
}
}
*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
bodyHandle->m_softBody = rsb;
rsb->setUserIndex2(*bodyUniqueId);
b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = *bodyUniqueId;
visualShape.m_linkIndex = -1;
visualShape.m_visualGeometryType = URDF_GEOM_MESH;
//dimensions just contains the scale
visualShape.m_dimensions[0] = 1;
visualShape.m_dimensions[1] = 1;
visualShape.m_dimensions[2] = 1;
//filename
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
//position and orientation
visualShape.m_localVisualFrame[0] = 0;
visualShape.m_localVisualFrame[1] = 0;
visualShape.m_localVisualFrame[2] = 0;
visualShape.m_localVisualFrame[3] = 0;
visualShape.m_localVisualFrame[4] = 0;
visualShape.m_localVisualFrame[5] = 0;
visualShape.m_localVisualFrame[6] = 1;
//color and ids to be set by the renderer
visualShape.m_rgbaColor[0] = 1;
visualShape.m_rgbaColor[1] = 1;
visualShape.m_rgbaColor[2] = 1;
visualShape.m_rgbaColor[3] = 1;
visualShape.m_tinyRendererTextureId = -1;
visualShape.m_textureUniqueId = -1;
visualShape.m_openglTextureId = -1;
if (meshData.m_gfxShape)
{
int texUid1 = -1;
if (meshData.m_textureHeight > 0 && meshData.m_textureWidth > 0 && meshData.m_textureImage1)
{
texUid1 = m_data->m_guiHelper->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
}
visualShape.m_openglTextureId = texUid1;
int shapeUid1 = m_data->m_guiHelper->registerGraphicsShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, &meshData.m_gfxShape->m_indices->at(0), meshData.m_gfxShape->m_numIndices, B3_GL_TRIANGLES, texUid1);
rsb->getCollisionShape()->setUserIndex(shapeUid1);
float position[4] = { 0,0,0,1 };
float orientation[4] = { 0,0,0,1 };
float color[4] = { 1,1,1,1 };
float scaling[4] = { 1,1,1,1 };
int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling);
rsb->setUserIndex(instanceUid);
if (m_data->m_enableTinyRenderer)
{
int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
visualShape.m_tinyRendererTextureId = texUid2;
int linkIndex = -1;
int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
visualShape,
&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
&meshData.m_gfxShape->m_indices->at(0),
meshData.m_gfxShape->m_numIndices,
B3_GL_TRIANGLES,
texUid2,
rsb->getBroadphaseHandle()->getUid(),
*bodyUniqueId,
linkIndex);
rsb->setUserIndex3(softBodyGraphicsShapeUid);
}
delete meshData.m_gfxShape;
meshData.m_gfxShape = 0;
}
else
{
//m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
btAlignedObjectArray<GLInstanceVertex> gfxVertices;
btAlignedObjectArray<int> indices;
int strideInBytes = 9 * sizeof(float);
gfxVertices.resize(rsb->m_faces.size() * 3);
for (int i = 0; i < rsb->m_faces.size(); i++) // Foreach face
{
for (int k = 0; k < 3; k++) // Foreach vertex on a face
{
int currentIndex = i * 3 + k;
for (int j = 0; j < 3; j++)
{
gfxVertices[currentIndex].xyzw[j] = rsb->m_faces[i].m_n[k]->m_x[j];
}
for (int j = 0; j < 3; j++)
{
gfxVertices[currentIndex].normal[j] = rsb->m_faces[i].m_n[k]->m_n[j];
}
for (int j = 0; j < 2; j++)
{
gfxVertices[currentIndex].uv[j] = btFabs(btFabs(10. * rsb->m_faces[i].m_n[k]->m_x[j]));
}
indices.push_back(currentIndex);
}
}
if (gfxVertices.size() && indices.size())
{
int red = 173;
int green = 199;
int blue = 255;
int texWidth = 256;
int texHeight = 256;
btAlignedObjectArray<unsigned char> texels;
texels.resize(texWidth* texHeight * 3);
for (int i = 0; i < texWidth * texHeight * 3; i++)
texels[i] = 255;
for (int i = 0; i < texWidth; i++)
{
for (int j = 0; j < texHeight; j++)
{
int a = i < texWidth / 2 ? 1 : 0;
int b = j < texWidth / 2 ? 1 : 0;
if (a == b)
{
texels[(i + j * texWidth) * 3 + 0] = red;
texels[(i + j * texWidth) * 3 + 1] = green;
texels[(i + j * texWidth) * 3 + 2] = blue;
}
}
}
int texId = m_data->m_guiHelper->registerTexture(&texels[0], texWidth, texHeight);
visualShape.m_openglTextureId = texId;
int shapeId = m_data->m_guiHelper->registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texId);
b3Assert(shapeId >= 0);
rsb->getCollisionShape()->setUserIndex(shapeId);
if (m_data->m_enableTinyRenderer)
{
int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(&texels[0], texWidth, texHeight);
visualShape.m_tinyRendererTextureId = texUid2;
int linkIndex = -1;
int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
visualShape,
&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texUid2,
rsb->getBroadphaseHandle()->getUid(),
*bodyUniqueId,
linkIndex);
rsb->setUserIndex3(softBodyGraphicsShapeUid);
}
}
}
btAlignedObjectArray<btVector3> vertices;
btAlignedObjectArray<btVector3> normals;
if (rsb->m_renderNodes.size() == 0)
{
rsb->m_renderNodes.resize(rsb->m_faces.size()*3);
vertices.resize(rsb->m_faces.size() * 3);
normals.resize(rsb->m_faces.size() * 3);
for (int i = 0; i < rsb->m_faces.size(); i++) // Foreach face
{
for (int k = 0; k < 3; k++) // Foreach vertex on a face
{
int currentIndex = i * 3 + k;
for (int j = 0; j < 3; j++)
{
rsb->m_renderNodes[currentIndex].m_x[j] = rsb->m_faces[i].m_n[k]->m_x[j];
}
for (int j = 0; j < 3; j++)
{
rsb->m_renderNodes[currentIndex].m_normal[j] = rsb->m_faces[i].m_n[k]->m_n[j];
}
for (int j = 0; j < 2; j++)
{
rsb->m_renderNodes[currentIndex].m_uv1[j] = btFabs(10*rsb->m_faces[i].m_n[k]->m_x[j]);
}
rsb->m_renderNodes[currentIndex].m_uv1[2] = 0;
vertices[currentIndex] = rsb->m_faces[i].m_n[k]->m_x;
normals[currentIndex] = rsb->m_faces[i].m_n[k]->m_n;
}
}
btSoftBodyHelpers::extrapolateBarycentricWeights(rsb);
}
else
{
vertices.resize(rsb->m_renderNodes.size());
normals.resize(rsb->m_renderNodes.size());
for (int i = 0; i < rsb->m_renderNodes.size(); i++) // Foreach face
{
vertices[i] = rsb->m_renderNodes[i].m_x;
normals[i] = rsb->m_renderNodes[i].m_normal;
}
}
m_data->m_pluginManager.getRenderInterface()->updateShape(rsb->getUserIndex3(), &vertices[0], vertices.size(), &normals[0], normals.size());
if (!reduced_deformable.m_name.empty())
{
bodyHandle->m_bodyName = reduced_deformable.m_name;
}
else
{
int pos = strlen(relativeFileName) - 1;
while (pos >= 0 && relativeFileName[pos] != '/')
{
pos--;
}
btAssert(strlen(relativeFileName) - pos - 5 > 0);
std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos));
bodyHandle->m_bodyName = object_name;
}
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
#endif
return true;
}
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
@ -11378,8 +10906,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
// deformWorld->getWorldInfo().m_gravity = grav;
deformWorld->setGravity(grav);
deformWorld->getWorldInfo().m_gravity = grav;
for (int i = 0; i < m_data->m_lf.size(); ++i)
{
btDeformableLagrangianForce* force = m_data->m_lf[i];

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@ -110,7 +110,6 @@ protected:
bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b);
bool processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision);
bool processReducedDeformable(const UrdfReducedDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -617,7 +617,6 @@ enum b3ResetSimulationFlags
RESET_USE_DEFORMABLE_WORLD=1,
RESET_USE_DISCRETE_DYNAMICS_WORLD=2,
RESET_USE_SIMPLE_BROADPHASE=4,
RESET_USE_REDUCED_DEFORMABLE_WORLD=8,
};
struct b3BodyNotificationArgs

27
examples/pybullet/examples/reduced_deformable_cube.py
View File

@ -1,27 +0,0 @@
import pybullet as p
from time import sleep
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
p.setGravity(0, 0, -10)
tex = p.loadTexture("uvmap.png")
planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [1,1,5],useMaximalCoordinates = True)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_cube.mp4")
cube = p.loadURDF("reduced_cube/reduced_cube.urdf", [1,1,1])
p.changeVisualShape(cube, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(0)
while p.isConnected():
p.stepSimulation()
p.getCameraImage(320,200)
p.setGravity(0,0,-10)

32
examples/pybullet/examples/reduced_deformable_torus.py
View File

@ -1,32 +0,0 @@
import pybullet as p
from time import sleep
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
p.setGravity(0, 0, -10)
tex = p.loadTexture("uvmap.png")
planeId = p.loadURDF("plane.urdf", [0,0,-2])
box1 = p.loadURDF("cube.urdf", [1,1,3],useMaximalCoordinates = True)
box2 = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
# p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_torus.mp4")
reduced_obj1= p.loadURDF("reduced_torus/reduced_torus.urdf", [1,1,1])
p.changeVisualShape(reduced_obj1, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
reduced_obj2 = p.loadURDF("reduced_torus/reduced_torus.urdf", [1,2,1])
p.changeVisualShape(reduced_obj2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(0)
while p.isConnected():
p.stepSimulation()
p.getCameraImage(320,200)
p.setGravity(0,0,-10)

1
examples/pybullet/pybullet.c
View File

@ -13253,7 +13253,6 @@ initpybullet(void)
//PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_BLOCK",eConstraintSolverLCP_BLOCK_PGS);
PyModule_AddIntConstant(m, "RESET_USE_DEFORMABLE_WORLD", RESET_USE_DEFORMABLE_WORLD);
PyModule_AddIntConstant(m, "RESET_USE_REDUCED_DEFORMABLE_WORLD", RESET_USE_REDUCED_DEFORMABLE_WORLD);
PyModule_AddIntConstant(m, "RESET_USE_DISCRETE_DYNAMICS_WORLD", RESET_USE_DISCRETE_DYNAMICS_WORLD);
PyModule_AddIntConstant(m, "RESET_USE_SIMPLE_BROADPHASE", RESET_USE_SIMPLE_BROADPHASE);

792
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp
View File

@ -1,792 +0,0 @@
#include "btReducedDeformableBody.h"
#include "../btSoftBodyInternals.h"
#include "btReducedDeformableBodyHelpers.h"
#include "LinearMath/btTransformUtil.h"
#include <iostream>
#include <fstream>
btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false)
{
// reduced deformable
m_reducedModel = true;
m_nReduced = 0;
m_nFull = 0;
m_nodeIndexOffset = 0;
m_transform_lock = false;
m_ksScale = 1.0;
m_rhoScale = 1.0;
// rigid motion
m_linearVelocity.setZero();
m_angularVelocity.setZero();
m_internalDeltaLinearVelocity.setZero();
m_internalDeltaAngularVelocity.setZero();
m_angularVelocityFromReduced.setZero();
m_internalDeltaAngularVelocityFromReduced.setZero();
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
// m_invInertiaLocal.setValue(1, 1, 1);
m_invInertiaLocal.setIdentity();
m_mass = 0.0;
m_inverseMass = 0.0;
m_linearDamping = 0;
m_angularDamping = 0;
// Rayleigh damping
m_dampingAlpha = 0;
m_dampingBeta = 0;
m_rigidTransformWorld.setIdentity();
}
void btReducedDeformableBody::setReducedModes(int num_modes, int full_size)
{
m_nReduced = num_modes;
m_nFull = full_size;
m_reducedDofs.resize(m_nReduced, 0);
m_reducedDofsBuffer.resize(m_nReduced, 0);
m_reducedVelocity.resize(m_nReduced, 0);
m_reducedVelocityBuffer.resize(m_nReduced, 0);
m_reducedForceElastic.resize(m_nReduced, 0);
m_reducedForceDamping.resize(m_nReduced, 0);
m_reducedForceExternal.resize(m_nReduced, 0);
m_internalDeltaReducedVelocity.resize(m_nReduced, 0);
m_nodalMass.resize(full_size, 0);
m_localMomentArm.resize(m_nFull);
}
void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array)
{
btScalar total_mass = 0;
btVector3 CoM(0, 0, 0);
for (int i = 0; i < m_nFull; ++i)
{
m_nodalMass[i] = m_rhoScale * mass_array[i];
m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0;
total_mass += m_rhoScale * mass_array[i];
CoM += m_nodalMass[i] * m_nodes[i].m_x;
}
// total rigid body mass
m_mass = total_mass;
m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
// original CoM
m_initialCoM = CoM / total_mass;
}
void btReducedDeformableBody::setInertiaProps()
{
// make sure the initial CoM is at the origin (0,0,0)
// for (int i = 0; i < m_nFull; ++i)
// {
// m_nodes[i].m_x -= m_initialCoM;
// }
// m_initialCoM.setZero();
m_rigidTransformWorld.setOrigin(m_initialCoM);
m_interpolationWorldTransform = m_rigidTransformWorld;
updateLocalInertiaTensorFromNodes();
// update world inertia tensor
btMatrix3x3 rotation;
rotation.setIdentity();
updateInitialInertiaTensor(rotation);
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
}
void btReducedDeformableBody::setRigidVelocity(const btVector3& v)
{
m_linearVelocity = v;
}
void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega)
{
m_angularVelocity = omega;
}
void btReducedDeformableBody::setStiffnessScale(const btScalar ks)
{
m_ksScale = ks;
}
void btReducedDeformableBody::setMassScale(const btScalar rho)
{
m_rhoScale = rho;
}
void btReducedDeformableBody::setFixedNodes(const int n_node)
{
m_fixedNodes.push_back(n_node);
m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver.
}
void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta)
{
m_dampingAlpha = alpha;
m_dampingBeta = beta;
}
void btReducedDeformableBody::internalInitialization()
{
// zeroing
endOfTimeStepZeroing();
// initialize rest position
updateRestNodalPositions();
// initialize local nodal moment arm form the CoM
updateLocalMomentArm();
// initialize projection matrix
updateExternalForceProjectMatrix(false);
}
void btReducedDeformableBody::updateLocalMomentArm()
{
TVStack delta_x;
delta_x.resize(m_nFull);
for (int i = 0; i < m_nFull; ++i)
{
for (int k = 0; k < 3; ++k)
{
// compute displacement
delta_x[i][k] = 0;
for (int j = 0; j < m_nReduced; ++j)
{
delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j];
}
}
// get new moment arm Sq + x0
m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i];
}
}
void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized)
{
// if not initialized, need to compute both P_A and Cq
// otherwise, only need to udpate Cq
if (!initialized)
{
// resize
m_projPA.resize(m_nReduced);
m_projCq.resize(m_nReduced);
m_STP.resize(m_nReduced);
m_MrInvSTP.resize(m_nReduced);
// P_A
for (int r = 0; r < m_nReduced; ++r)
{
m_projPA[r].resize(3 * m_nFull, 0);
for (int i = 0; i < m_nFull; ++i)
{
btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass);
btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
btVector3 prod_i = mass_scaled_i * s_ri;
for (int k = 0; k < 3; ++k)
m_projPA[r][3 * i + k] = prod_i[k];
// btScalar ratio = m_nodalMass[i] / m_mass;
// m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio,
// - m_modes[r][3 * i + 1] * ratio,
// - m_modes[r][3 * i + 2] * ratio);
}
}
}
// C(q) is updated once per position update
for (int r = 0; r < m_nReduced; ++r)
{
m_projCq[r].resize(3 * m_nFull, 0);
for (int i = 0; i < m_nFull; ++i)
{
btMatrix3x3 r_star = Cross(m_localMomentArm[i]);
btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri;
for (int k = 0; k < 3; ++k)
m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k];
// btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
// m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]);
}
}
}
void btReducedDeformableBody::endOfTimeStepZeroing()
{
for (int i = 0; i < m_nReduced; ++i)
{
m_reducedForceElastic[i] = 0;
m_reducedForceDamping[i] = 0;
m_reducedForceExternal[i] = 0;
m_internalDeltaReducedVelocity[i] = 0;
m_reducedDofsBuffer[i] = m_reducedDofs[i];
m_reducedVelocityBuffer[i] = m_reducedVelocity[i];
}
// std::cout << "zeroed!\n";
}
void btReducedDeformableBody::applyInternalVelocityChanges()
{
m_linearVelocity += m_internalDeltaLinearVelocity;
m_angularVelocity += m_internalDeltaAngularVelocity;
m_internalDeltaLinearVelocity.setZero();
m_internalDeltaAngularVelocity.setZero();
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r];
m_internalDeltaReducedVelocity[r] = 0;
}
}
void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
{
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
}
void btReducedDeformableBody::updateReducedDofs(btScalar solverdt)
{
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r];
}
}
void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans)
{
btVector3 origin = ref_trans.getOrigin();
btMatrix3x3 rotation = ref_trans.getBasis();
for (int i = 0; i < m_nFull; ++i)
{
m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin;
m_nodes[i].m_q = m_nodes[i].m_x;
}
}
void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt)
{
// update reduced velocity
for (int r = 0; r < m_nReduced; ++r)
{
// the reduced mass is always identity!
btScalar delta_v = 0;
delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
// delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v;
}
}
void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans)
{
// compute the reduced contribution to the angular velocity
// btVector3 sum_linear(0, 0, 0);
// btVector3 sum_angular(0, 0, 0);
// m_linearVelocityFromReduced.setZero();
// m_angularVelocityFromReduced.setZero();
// for (int i = 0; i < m_nFull; ++i)
// {
// btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
// btMatrix3x3 r_star = Cross(r_com);
// btVector3 v_from_reduced(0, 0, 0);
// for (int k = 0; k < 3; ++k)
// {
// for (int r = 0; r < m_nReduced; ++r)
// {
// v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
// }
// }
// btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
// btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
// sum_linear += delta_linear;
// sum_angular += delta_angular;
// // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
// // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
// // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
// // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
// }
// m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
// m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
// m_linearVelocity -= m_linearVelocityFromReduced;
// m_angularVelocity -= m_angularVelocityFromReduced;
for (int i = 0; i < m_nFull; ++i)
{
m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
}
}
const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const
{
btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity;
btVector3 L_reduced(0, 0, 0);
btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced);
for (int i = 0; i < m_nFull; ++i)
{
btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i];
btMatrix3x3 r_star = Cross(r_com);
btVector3 v_from_reduced(0, 0, 0);
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
}
}
L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com));
// L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
}
return L_rigid + L_reduced;
}
const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const
{
btVector3 v_from_reduced(0, 0, 0);
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
// compute velocity contributed by the reduced velocity
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r];
}
}
// get new velocity
btVector3 vel = m_angularVelocity.cross(r_com) +
ref_trans.getBasis() * v_from_reduced +
m_linearVelocity;
return vel;
}
const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const
{
btVector3 deltaV_from_reduced(0, 0, 0);
btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
// compute velocity contributed by the reduced velocity
for (int k = 0; k < 3; ++k)
{
for (int r = 0; r < m_nReduced; ++r)
{
deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r];
}
}
// get delta velocity
btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) +
ref_trans.getBasis() * deltaV_from_reduced +
m_internalDeltaLinearVelocity;
return deltaV;
}
void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step)
{
btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform);
updateInertiaTensor();
// m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose();
m_rigidTransformWorld = m_interpolationWorldTransform;
m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld;
}
void btReducedDeformableBody::transformTo(const btTransform& trs)
{
btTransform current_transform = getRigidTransform();
btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
trs.getOrigin() - current_transform.getOrigin());
transform(new_transform);
}
void btReducedDeformableBody::transform(const btTransform& trs)
{
m_transform_lock = true;
// transform mesh
{
const btScalar margin = getCollisionShape()->getMargin();
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
btVector3 CoM = m_rigidTransformWorld.getOrigin();
btVector3 translation = trs.getOrigin();
btMatrix3x3 rotation = trs.getBasis();
for (int i = 0; i < m_nodes.size(); ++i)
{
Node& n = m_nodes[i];
n.m_x = rotation * (n.m_x - CoM) + CoM + translation;
n.m_q = rotation * (n.m_q - CoM) + CoM + translation;
n.m_n = rotation * n.m_n;
vol = btDbvtVolume::FromCR(n.m_x, margin);
m_ndbvt.update(n.m_leaf, vol);
}
updateNormals();
updateBounds();
updateConstants();
}
// update modes
updateModesByRotation(trs.getBasis());
// update inertia tensor
updateInitialInertiaTensor(trs.getBasis());
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
// update rigid frame (No need to update the rotation. Nodes have already been updated.)
m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin());
m_interpolationWorldTransform = m_rigidTransformWorld;
m_initialCoM = m_rigidTransformWorld.getOrigin();
internalInitialization();
}
void btReducedDeformableBody::scale(const btVector3& scl)
{
// Scaling the mesh after transform is applied is not allowed
btAssert(!m_transform_lock);
// scale the mesh
{
const btScalar margin = getCollisionShape()->getMargin();
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
btVector3 CoM = m_rigidTransformWorld.getOrigin();
for (int i = 0; i < m_nodes.size(); ++i)
{
Node& n = m_nodes[i];
n.m_x = (n.m_x - CoM) * scl + CoM;
n.m_q = (n.m_q - CoM) * scl + CoM;
vol = btDbvtVolume::FromCR(n.m_x, margin);
m_ndbvt.update(n.m_leaf, vol);
}
updateNormals();
updateBounds();
updateConstants();
initializeDmInverse();
}
// update inertia tensor
updateLocalInertiaTensorFromNodes();
btMatrix3x3 id;
id.setIdentity();
updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
internalInitialization();
}
void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces)
{
// Changing the total mass after transform is applied is not allowed
btAssert(!m_transform_lock);
btScalar scale_ratio = mass / m_mass;
// update nodal mass
for (int i = 0; i < m_nFull; ++i)
{
m_nodalMass[i] *= scale_ratio;
}
m_mass = mass;
m_inverseMass = mass > 0 ? 1.0 / mass : 0;
// update inertia tensors
updateLocalInertiaTensorFromNodes();
btMatrix3x3 id;
id.setIdentity();
updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed
updateInertiaTensor();
m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
internalInitialization();
}
void btReducedDeformableBody::updateRestNodalPositions()
{
// update reset nodal position
m_x0.resize(m_nFull);
for (int i = 0; i < m_nFull; ++i)
{
m_x0[i] = m_nodes[i].m_x;
}
}
// reference notes:
// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf
void btReducedDeformableBody::updateLocalInertiaTensorFromNodes()
{
btMatrix3x3 inertia_tensor;
inertia_tensor.setZero();
for (int p = 0; p < m_nFull; ++p)
{
btMatrix3x3 particle_inertia;
particle_inertia.setZero();
btVector3 r = m_nodes[p].m_x - m_initialCoM;
particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]);
particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]);
particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]);
particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]);
particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]);
particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]);
particle_inertia[1][0] = particle_inertia[0][1];
particle_inertia[2][0] = particle_inertia[0][2];
particle_inertia[2][1] = particle_inertia[1][2];
inertia_tensor += particle_inertia;
}
m_invInertiaLocal = inertia_tensor.inverse();
}
void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
{
// m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose();
}
void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation)
{
for (int r = 0; r < m_nReduced; ++r)
{
for (int i = 0; i < m_nFull; ++i)
{
btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
nodal_disp = rotation * nodal_disp;
for (int k = 0; k < 3; ++k)
{
m_modes[r][3 * i + k] = nodal_disp[k];
}
}
}
}
void btReducedDeformableBody::updateInertiaTensor()
{
m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose();
}
void btReducedDeformableBody::applyDamping(btScalar timeStep)
{
m_linearVelocity *= btScalar(1) - m_linearDamping;
m_angularDamping *= btScalar(1) - m_angularDamping;
}
void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse)
{
m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_linearVelocity);
#endif
}
void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque)
{
m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_angularVelocity);
#endif
}
void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass == btScalar(0.))
{
std::cout << "something went wrong...probably didn't initialize?\n";
btAssert(false);
}
// delta linear velocity
m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass;
// delta angular velocity
btVector3 torque = rel_pos.cross(impulse * m_linearFactor);
m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
}
btVector3 btReducedDeformableBody::getRelativePos(int n_node)
{
btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
btVector3 ri = rotation * m_localMomentArm[n_node];
return ri;
}
btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node)
{
// relative position
btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
btVector3 ri = rotation * m_localMomentArm[n_node];
btMatrix3x3 ri_skew = Cross(ri);
// calculate impulse factor
// rigid part
btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0);
btMatrix3x3 K1 = Diagonal(inv_mass);
K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew;
// reduced deformable part
btMatrix3x3 SA;
SA.setZero();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
for (int r = 0; r < m_nReduced; ++r)
{
SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
}
}
}
btMatrix3x3 RSARinv = rotation * SA * rotation.transpose();
TVStack omega_helper; // Sum_i m_i r*_i R S_i
omega_helper.resize(m_nReduced);
for (int r = 0; r < m_nReduced; ++r)
{
omega_helper[r].setZero();
for (int i = 0; i < m_nFull; ++i)
{
btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i];
btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
omega_helper[r] += mi_rstar_i * rotation * s_ri;
}
}
btMatrix3x3 sum_multiply_A;
sum_multiply_A.setZero();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
for (int r = 0; r < m_nReduced; ++r)
{
sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
}
}
}
btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
return m_rigidOnly ? K1 : K1 + K2;
}
void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
{
if (!m_rigidOnly)
{
// apply impulse force
applyFullSpaceNodalForce(impulse / dt, n_node);
// update delta damping force
tDenseArray reduced_vel_tmp;
reduced_vel_tmp.resize(m_nReduced);
for (int r = 0; r < m_nReduced; ++r)
{
reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r];
}
applyReducedDampingForce(reduced_vel_tmp);
// applyReducedDampingForce(m_internalDeltaReducedVelocity);
// delta reduced velocity
for (int r = 0; r < m_nReduced; ++r)
{
// The reduced mass is always identity!
m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
}
}
internalApplyRigidImpulse(impulse, rel_pos);
}
void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node)
{
// f_local = R^-1 * f_ext //TODO: interpoalted transfrom
// btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext;
// f_ext_r = [S^T * P]_{n_node} * f_local
tDenseArray f_ext_r;
f_ext_r.resize(m_nReduced, 0);
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedForceExternal[r] = 0;
for (int k = 0; k < 3; ++k)
{
f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
}
m_reducedForceExternal[r] += f_ext_r[r];
}
}
void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
{
// update rigid frame velocity
m_linearVelocity += dt * gravity;
}
void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs)
{
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r];
}
}
void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
{
for (int r = 0; r < m_nReduced; ++r)
{
m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r];
}
}
btScalar btReducedDeformableBody::getTotalMass() const
{
return m_mass;
}
btTransform& btReducedDeformableBody::getRigidTransform()
{
return m_rigidTransformWorld;
}
const btVector3& btReducedDeformableBody::getLinearVelocity() const
{
return m_linearVelocity;
}
const btVector3& btReducedDeformableBody::getAngularVelocity() const
{
return m_angularVelocity;
}
void btReducedDeformableBody::disableReducedModes(const bool rigid_only)
{
m_rigidOnly = rigid_only;
}
bool btReducedDeformableBody::isReducedModesOFF() const
{
return m_rigidOnly;
}

257
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h
View File

@ -1,257 +0,0 @@
#ifndef BT_REDUCED_SOFT_BODY_H
#define BT_REDUCED_SOFT_BODY_H
#include "../btSoftBody.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
// Reduced deformable body is a simplified deformable object embedded in a rigid frame.
class btReducedDeformableBody : public btSoftBody
{
public:
//
// Typedefs
//
typedef btAlignedObjectArray<btVector3> TVStack;
// typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
typedef btAlignedObjectArray<btScalar> tDenseArray;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
private:
// flag to turn off the reduced modes
bool m_rigidOnly;
// Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
bool m_transform_lock;
// scaling factors
btScalar m_rhoScale; // mass density scale
btScalar m_ksScale; // stiffness scale
// projection matrix
tDenseMatrix m_projPA; // Eqn. 4.11 from Rahul Sheth's thesis
tDenseMatrix m_projCq;
tDenseArray m_STP;
tDenseArray m_MrInvSTP;
TVStack m_localMomentArm; // Sq + x0
btVector3 m_internalDeltaLinearVelocity;
btVector3 m_internalDeltaAngularVelocity;
tDenseArray m_internalDeltaReducedVelocity;
btVector3 m_linearVelocityFromReduced; // contribution to the linear velocity from reduced velocity
btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
btVector3 m_internalDeltaAngularVelocityFromReduced;
protected:
// rigid frame
btScalar m_mass; // total mass of the rigid frame
btScalar m_inverseMass; // inverse of the total mass of the rigid frame
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btScalar m_linearDamping; // linear damping coefficient
btScalar m_angularDamping; // angular damping coefficient
btVector3 m_linearFactor;
btVector3 m_angularFactor;
// btVector3 m_invInertiaLocal;
btMatrix3x3 m_invInertiaLocal;
btTransform m_rigidTransformWorld;
btMatrix3x3 m_invInertiaTensorWorldInitial;
btMatrix3x3 m_invInertiaTensorWorld;
btMatrix3x3 m_interpolateInvInertiaTensorWorld;
btVector3 m_initialCoM; // initial center of mass (original of the m_rigidTransformWorld)
// damping
btScalar m_dampingAlpha;
btScalar m_dampingBeta;
public:
//
// Fields
//
// reduced space
int m_nReduced;
int m_nFull;
tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
tDenseArray m_reducedDofs; // Reduced degree of freedom
tDenseArray m_reducedDofsBuffer; // Reduced degree of freedom at t^n
tDenseArray m_reducedVelocity; // Reduced velocity array
tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n
tDenseArray m_reducedForceExternal; // reduced external force
tDenseArray m_reducedForceElastic; // reduced internal elastic force
tDenseArray m_reducedForceDamping; // reduced internal damping force
tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model
tDenseArray m_Kr; // reduced stiffness matrix
// full space
TVStack m_x0; // Rest position
tDenseArray m_nodalMass; // Mass on each node
btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
int m_nodeIndexOffset; // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
// contacts
btAlignedObjectArray<int> m_contactNodesList;
//
// Api
//
btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
~btReducedDeformableBody() {}
//
// initializing helpers
//
void internalInitialization();
void setReducedModes(int num_modes, int full_size);
void setMassProps(const tDenseArray& mass_array);
void setInertiaProps();
void setRigidVelocity(const btVector3& v);
void setRigidAngularVelocity(const btVector3& omega);
void setStiffnessScale(const btScalar ks);
void setMassScale(const btScalar rho);
void setFixedNodes(const int n_node);
void setDamping(const btScalar alpha, const btScalar beta);
void disableReducedModes(const bool rigid_only);
virtual void setTotalMass(btScalar mass, bool fromfaces = false);
//
// various internal updates
//
virtual void transformTo(const btTransform& trs);
virtual void transform(const btTransform& trs);
// caution:
// need to use scale before using transform, because the scale is performed in the local frame
// (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
virtual void scale(const btVector3& scl);
private:
void updateRestNodalPositions();
void updateInitialInertiaTensor(const btMatrix3x3& rotation);
void updateLocalInertiaTensorFromNodes();
void updateInertiaTensor();
void updateModesByRotation(const btMatrix3x3& rotation);
public:
void updateLocalMomentArm();
void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
// update the external force projection matrix
void updateExternalForceProjectMatrix(bool initialized);
void endOfTimeStepZeroing();
void applyInternalVelocityChanges();
//
// position and velocity update related
//
// compute reduced degree of freedoms
void updateReducedDofs(btScalar solverdt);
// compute reduced velocity update (for explicit time stepping)
void updateReducedVelocity(btScalar solverdt);
// map to full degree of freedoms
void mapToFullPosition(const btTransform& ref_trans);
// compute full space velocity from the reduced velocity
void mapToFullVelocity(const btTransform& ref_trans);
// compute total angular momentum
const btVector3 computeTotalAngularMomentum() const;
// get a single node's full space velocity from the reduced velocity
const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
// get a single node's all delta velocity
const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
//
// rigid motion related
//
void applyDamping(btScalar timeStep);
void applyCentralImpulse(const btVector3& impulse);
void applyTorqueImpulse(const btVector3& torque);
void proceedToTransform(btScalar dt, bool end_of_time_step);
//
// force related
//
// apply impulse to the rigid frame
void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
// apply impulse to nodes in the full space
void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
// apply nodal external force in the full space
void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
// apply gravity to the rigid frame
void applyRigidGravity(const btVector3& gravity, btScalar dt);
// apply reduced elastic force
void applyReducedElasticForce(const tDenseArray& reduce_dofs);
// apply reduced damping force
void applyReducedDampingForce(const tDenseArray& reduce_vel);
// calculate the impulse factor
virtual btMatrix3x3 getImpulseFactor(int n_node);
// get relative position from a node to the CoM of the rigid frame
btVector3 getRelativePos(int n_node);
//
// accessors
//
bool isReducedModesOFF() const;
btScalar getTotalMass() const;
btTransform& getRigidTransform();
const btVector3& getLinearVelocity() const;
const btVector3& getAngularVelocity() const;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
void clampVelocity(btVector3& v) const {
v.setX(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
);
v.setY(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
);
v.setZ(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
);
}
#endif
};
#endif // BT_REDUCED_SOFT_BODY_H

215
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp
View File

@ -1,215 +0,0 @@
#include "btReducedDeformableBodyHelpers.h"
#include "../btSoftBodyHelpers.h"
#include <iostream>
#include <string>
#include <sstream>
btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) {
std::string filename = file_path + vtk_file;
btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
rsb->setReducedModes(num_modes, rsb->m_nodes.size());
btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str());
rsb->disableReducedModes(rigid_only);
return rsb;
}
btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
{
std::ifstream fs;
fs.open(vtk_file);
btAssert(fs);
typedef btAlignedObjectArray<int> Index;
std::string line;
btAlignedObjectArray<btVector3> X;
btVector3 position;
btAlignedObjectArray<Index> indices;
bool reading_points = false;
bool reading_tets = false;
size_t n_points = 0;
size_t n_tets = 0;
size_t x_count = 0;
size_t indices_count = 0;
while (std::getline(fs, line))
{
std::stringstream ss(line);
if (line.size() == (size_t)(0))
{
}
else if (line.substr(0, 6) == "POINTS")
{
reading_points = true;
reading_tets = false;
ss.ignore(128, ' '); // ignore "POINTS"
ss >> n_points;
X.resize(n_points);
}
else if (line.substr(0, 5) == "CELLS")
{
reading_points = false;
reading_tets = true;
ss.ignore(128, ' '); // ignore "CELLS"
ss >> n_tets;
indices.resize(n_tets);
}
else if (line.substr(0, 10) == "CELL_TYPES")
{
reading_points = false;
reading_tets = false;
}
else if (reading_points)
{
btScalar p;
ss >> p;
position.setX(p);
ss >> p;
position.setY(p);
ss >> p;
position.setZ(p);
//printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
X[x_count++] = position;
}
else if (reading_tets)
{
int d;
ss >> d;
if (d != 4)
{
printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
fs.close();
return 0;
}
ss.ignore(128, ' '); // ignore "4"
Index tet;
tet.resize(4);
for (size_t i = 0; i < 4; i++)
{
ss >> tet[i];
//printf("%d ", tet[i]);
}
//printf("\n");
indices[indices_count++] = tet;
}
}
btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0);
for (int i = 0; i < n_tets; ++i)
{
const Index& ni = indices[i];
rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
{
rsb->appendLink(ni[0], ni[1], 0, true);
rsb->appendLink(ni[1], ni[2], 0, true);
rsb->appendLink(ni[2], ni[0], 0, true);
rsb->appendLink(ni[0], ni[3], 0, true);
rsb->appendLink(ni[1], ni[3], 0, true);
rsb->appendLink(ni[2], ni[3], 0, true);
}
}
btSoftBodyHelpers::generateBoundaryFaces(rsb);
rsb->initializeDmInverse();
rsb->m_tetraScratches.resize(rsb->m_tetras.size());
rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size());
printf("Nodes: %u\r\n", rsb->m_nodes.size());
printf("Links: %u\r\n", rsb->m_links.size());
printf("Faces: %u\r\n", rsb->m_faces.size());
printf("Tetras: %u\r\n", rsb->m_tetras.size());
fs.close();
return rsb;
}
void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path)
{
// read in eigenmodes, stiffness and mass matrices
std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin";
btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str());
std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin";
btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr, rsb->m_nReduced, Kr_file.c_str());
// std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin";
// btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str());
std::string modes_file = std::string(file_path) + "modes.bin";
btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str());
// read in full nodal mass
std::string M_file = std::string(file_path) + "M_diag_mat.bin";
btAlignedObjectArray<btScalar> mass_array;
btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str());
rsb->setMassProps(mass_array);
// calculate the inertia tensor in the local frame
rsb->setInertiaProps();
// other internal initialization
rsb->internalInitialization();
}
// read in a vector from the binary file
void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec,
const unsigned int n_size, // #entries read
const char* file)
{
std::ifstream f_in(file, std::ios::in | std::ios::binary);
// first get size
unsigned int size;
f_in.read((char*)&size, sizeof(uint32_t));
btAssert(size >= n_size); // make sure the #requested mode is smaller than the #available modes
// read data
vec.resize(n_size);
double temp;
for (unsigned int i = 0; i < n_size; ++i)
{
f_in.read((char*)&temp, sizeof(double));
vec[i] = btScalar(temp);
}
f_in.close();
}
// read in a matrix from the binary file
void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat,
const unsigned int n_modes, // #modes, outer array size
const unsigned int n_full, // inner array size
const char* file)
{
std::ifstream f_in(file, std::ios::in | std::ios::binary);
// first get size
unsigned int v_size;
f_in.read((char*)&v_size, sizeof(uint32_t));
btAssert(v_size >= n_modes * n_full); // make sure the #requested mode is smaller than the #available modes
// read data
mat.resize(n_modes);
for (int i = 0; i < n_modes; ++i)
{
for (int j = 0; j < n_full; ++j)
{
double temp;
f_in.read((char*)&temp, sizeof(double));
if (mat[i].size() != n_modes)
mat[i].resize(n_full);
mat[i][j] = btScalar(temp);
}
}
f_in.close();
}
void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin)
{
btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]);
btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]);
btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]);
inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + lz * lz),
mass / (btScalar(12.0)) * (lx * lx + ly * ly));
}

25
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h
View File

@ -1,25 +0,0 @@
#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H
#define BT_REDUCED_SOFT_BODY_HELPERS_H
#include "btReducedDeformableBody.h"
#include <string>
struct btReducedDeformableBodyHelpers
{
// create a reduced deformable object
static btReducedDeformableBody* createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only);
// read in geometry info from Vtk file
static btReducedDeformableBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
// read in all reduced files
static void readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path);
// read in a binary vector
static void readBinaryVec(btReducedDeformableBody::tDenseArray& vec, const unsigned int n_size, const char* file);
// read in a binary matrix
static void readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, const unsigned int n_modes, const unsigned int n_full, const char* file);
// calculate the local inertia tensor for a box shape reduced deformable object
static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin);
};
#endif // BT_REDUCED_SOFT_BODY_HELPERS_H

325
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
View File

@ -1,325 +0,0 @@
#include "btReducedDeformableBodySolver.h"
#include "btReducedDeformableBody.h"
btReducedDeformableBodySolver::btReducedDeformableBodySolver()
{
m_ascendOrder = true;
m_reducedSolver = true;
m_dampingAlpha = 0;
m_dampingBeta = 0;
m_gravity = btVector3(0, 0, 0);
}
void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
}
void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
{
m_softBodies.copyFromArray(bodies);
bool nodeUpdated = updateNodes();
if (nodeUpdated)
{
m_dv.resize(m_numNodes, btVector3(0, 0, 0));
m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
m_residual.resize(m_numNodes, btVector3(0, 0, 0));
m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
}
// need to setZero here as resize only set value for newly allocated items
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i].setZero();
m_ddv[i].setZero();
m_residual[i].setZero();
}
if (dt > 0)
{
m_dt = dt;
}
m_objective->reinitialize(nodeUpdated, dt);
int N = bodies.size();
if (nodeUpdated)
{
m_staticConstraints.resize(N);
m_nodeRigidConstraints.resize(N);
// m_faceRigidConstraints.resize(N);
}
for (int i = 0; i < N; ++i)
{
m_staticConstraints[i].clear();
m_nodeRigidConstraints[i].clear();
// m_faceRigidConstraints[i].clear();
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
rsb->m_contactNodesList.clear();
}
// set node index offsets
int sum = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
rsb->m_nodeIndexOffset = sum;
sum += rsb->m_nodes.size();
}
btDeformableBodySolver::updateSoftBodies();
}
void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
{
applyExplicitForce(solverdt);
// predict new mesh location
predictReduceDeformableMotion(solverdt);
//TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
}
void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
if (!rsb->isActive())
{
continue;
}
// clear contacts variables
rsb->m_nodeRigidContacts.resize(0);
rsb->m_faceRigidContacts.resize(0);
rsb->m_faceNodeContacts.resize(0);
// calculate inverse mass matrix for all nodes
for (int j = 0; j < rsb->m_nodes.size(); ++j)
{
if (rsb->m_nodes[j].m_im > 0)
{
rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
}
}
// rigid motion: t, R at time^*
rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
// update reduced dofs at time^*
// rsb->updateReducedDofs(solverdt);
// update local moment arm at time^*
// rsb->updateLocalMomentArm();
// rsb->updateExternalForceProjectMatrix(true);
// predict full space velocity at time^* (needed for constraints)
rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
// update full space nodal position at time^*
rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
// update bounding box
rsb->updateBounds();
// update tree
rsb->updateNodeTree(true, true);
if (!rsb->m_fdbvt.empty())
{
rsb->updateFaceTree(true, true);
}
}
}
void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
// apply gravity to the rigid frame, get m_linearVelocity at time^*
rsb->applyRigidGravity(m_gravity, solverdt);
if (!rsb->isReducedModesOFF())
{
// add internal force (elastic force & damping force)
rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
// get reduced velocity at time^*
rsb->updateReducedVelocity(solverdt);
}
// apply damping (no need at this point)
// rsb->applyDamping(solverdt);
}
}
void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
// rigid motion
rsb->proceedToTransform(timeStep, true);
if (!rsb->isReducedModesOFF())
{
// update reduced dofs for time^n+1
rsb->updateReducedDofs(timeStep);
// update local moment arm for time^n+1
rsb->updateLocalMomentArm();
rsb->updateExternalForceProjectMatrix(true);
}
// update mesh nodal positions for time^n+1
rsb->mapToFullPosition(rsb->getRigidTransform());
// update mesh nodal velocity
rsb->mapToFullVelocity(rsb->getRigidTransform());
// end of time step clean up and update
rsb->endOfTimeStepZeroing();
// update the rendering mesh
rsb->interpolateRenderMesh();
}
}
void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
if (!rsb->isActive())
{
continue;
}
// set fixed constraints
for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
{
int i_node = rsb->m_fixedNodes[j];
if (rsb->m_nodes[i_node].m_im == 0)
{
for (int k = 0; k < 3; ++k)
{
btVector3 dir(0, 0, 0);
dir[k] = 1;
btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
m_staticConstraints[i].push_back(static_constraint);
}
}
}
btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
// set Deformable Node vs. Rigid constraint
for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
// skip fixed points
if (contact.m_node->m_im == 0)
{
continue;
}
btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
m_nodeRigidConstraints[i].push_back(constraint);
rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
}
// std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
// std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
}
}
btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderContactConstraintPool;
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
// shuffle the order of applying constraint
m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
// fixed constraint order
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
}
// contact constraint order
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
}
m_ascendOrder = m_ascendOrder ? false : true;
}
else
{
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
m_orderNonContactConstraintPool[j] = j;
}
// contact constraint order
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
m_orderContactConstraintPool[j] = j;
}
}
// handle fixed constraint
for (int k = 0; k < m_staticConstraints[i].size(); ++k)
{
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// handle contact constraint
// node vs rigid contact
// std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
{
btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face vs rigid contact
// for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
// {
// btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
// btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
// residualSquare = btMax(residualSquare, localResidualSquare);
// }
}
return residualSquare;
}
void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
{
// reduced deformable update
for (int i = 0; i < m_softBodies.size(); ++i)
{
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
rsb->applyInternalVelocityChanges();
}
m_ascendOrder = true;
}

61
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h
View File

@ -1,61 +0,0 @@
#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "btReducedDeformableContactConstraint.h"
class btReducedDeformableBody;
class btReducedDeformableBodySolver : public btDeformableBodySolver
{
protected:
bool m_ascendOrder;
btScalar m_dampingAlpha;
btScalar m_dampingBeta;
btVector3 m_gravity;
void predictReduceDeformableMotion(btScalar solverdt);
void applyExplicitForce(btScalar solverdt);
public:
btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableStaticConstraint> > m_staticConstraints;
btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
btReducedDeformableBodySolver();
~btReducedDeformableBodySolver() {}
virtual void setGravity(const btVector3& gravity);
virtual SolverTypes getSolverType() const
{
return REDUCED_DEFORMABLE_SOLVER;
}
// resize/clear data structures
virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt);
virtual void predictMotion(btScalar solverdt);
virtual void applyTransforms(btScalar timeStep);
// set up contact constraints
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
// solve all constraints (fixed and contact)
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
// apply all the delta velocities
virtual void deformableBodyInternalWriteBack();
// virtual void setProjection() {}
// virtual void setLagrangeMultiplier() {}
// virtual void setupDeformableSolve(bool implicit);
};
#endif // BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H

579
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp
View File

@ -1,579 +0,0 @@
#include "btReducedDeformableContactConstraint.h"
#include <iostream>
// ================= static constraints ===================
btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
btReducedDeformableBody* rsb,
btSoftBody::Node* node,
const btVector3& ri,
const btVector3& x0,
const btVector3& dir,
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
{
m_erp = 0.2;
m_appliedImpulse = 0;
// get impulse factor
m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
}
btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
// target velocity of fixed constraint is 0
btVector3 deltaVa = getDeltaVa();
btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
m_appliedImpulse = m_appliedImpulse + deltaImpulse;
btVector3 impulse = deltaImpulse * m_impulseDirection;
applyImpulse(impulse);
// calculate residual
btScalar residualSquare = m_impulseFactor * deltaImpulse;
residualSquare *= residualSquare;
return residualSquare;
}
// this calls reduced deformable body's internalApplyFullSpaceImpulse
void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
{
// apply full space impulse
m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
}
btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
{
return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
}
// ================= base contact constraints ===================
btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
btReducedDeformableBody* rsb,
const btSoftBody::DeformableRigidContact& c,
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
{
m_nodeQueryIndex = 0;
m_appliedNormalImpulse = 0;
m_appliedTangentImpulse = 0;
m_rhs = 0;
m_rhs_tangent = 0;
m_cfm = infoGlobal.m_deformable_cfm;
m_cfm_friction = 0;
m_erp = infoGlobal.m_deformable_erp;
m_erp_friction = infoGlobal.m_deformable_erp;
m_friction = infoGlobal.m_friction;
m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
}
void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
{
if (!m_collideMultibody)
{
m_solverBodyId = bodyId;
m_solverBody = &solver_body;
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_contactTangent);
m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
}
}
btVector3 btReducedDeformableRigidContactConstraint::getVa() const
{
btVector3 Va(0, 0, 0);
if (!m_collideStatic)
{
Va = btDeformableRigidContactConstraint::getVa();
}
return Va;
}
btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
// btVector3 Va = getVa();
// btVector3 deltaVa = Va - m_bufferVelocityA;
// if (!m_collideStatic)
// {
// std::cout << "moving collision!!!\n";
// std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
// std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
// << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
// << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
// }
btVector3 deltaVa = getDeltaVa();
btVector3 deltaVb = getDeltaVb();
// if (!m_collideStatic)
// {
// std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
// std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
// }
// get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
btVector3 deltaV_rel = deltaVa - deltaVb;
btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
// if (!m_collideStatic)
// {
// std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
// 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 - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
// if (!m_collideStatic)
// {
// std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n";
// std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
// }
{
// cumulative impulse that has been applied
btScalar sum = m_appliedNormalImpulse + deltaImpulse;
// if the cumulative impulse is pushing the object into the rigid body, set it zero
if (sum < 0)
{
deltaImpulse = -m_appliedNormalImpulse;
m_appliedNormalImpulse = 0;
}
else
{
m_appliedNormalImpulse = sum;
}
}
// if (!m_collideStatic)
// {
// std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
// std::cout << "deltaImpulse: " << deltaImpulse << '\n';
// }
// residual is the nodal normal velocity change in current iteration
btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual
residualSquare *= residualSquare;
// apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
btScalar deltaImpulse_tangent = 0;
btScalar deltaImpulse_tangent2 = 0;
{
// calculate how much impulse is needed
// btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
// btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
// deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
// btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
btScalar upper_limit = m_appliedNormalImpulse * m_friction;
// if (sum > upper_limit)
// {
// deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
// m_appliedTangentImpulse = upper_limit;
// }
// else if (sum < lower_limit)
// {
// deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
// m_appliedTangentImpulse = lower_limit;
// }
// else
// {
// m_appliedTangentImpulse = sum;
// }
//
calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
if (m_collideMultibody)
{
calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
}
if (!m_collideStatic)
{
// std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n";
// std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\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_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (!m_collideStatic)
{
// std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
// << m_linearComponentNormal[1] << '\t'
// << m_linearComponentNormal[2] << '\n';
// std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
// << m_angularComponentNormal[1] << '\t'
// << m_angularComponentNormal[2] << '\n';
if (!m_collideMultibody) // collision with rigid body
{
// std::cout << "rigid impulse applied!!\n";
// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
// << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
// << 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";
// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
// << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
// << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
}
else // collision with multibody
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
// const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
// std::cout << "deltaV_normal: ";
// for (int i = 0; i < ndof; ++i)
// {
// std::cout << i << "\t" << deltaV_normal[i] << '\n';
// }
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent);
const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
}
}
}
}
return residualSquare;
}
void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent,
btScalar& appliedImpulse,
const btScalar rhs_tangent,
const btScalar tangentImpulseFactorInv,
const btVector3& tangent,
const btScalar lower_limit,
const btScalar upper_limit,
const btVector3& deltaV_rel)
{
btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
btScalar sum = appliedImpulse + deltaImpulse_tangent;
if (sum > upper_limit)
{
deltaImpulse_tangent = upper_limit - appliedImpulse;
appliedImpulse = upper_limit;
}
else if (sum < lower_limit)
{
deltaImpulse_tangent = lower_limit - appliedImpulse;
appliedImpulse = lower_limit;
}
else
{
appliedImpulse = sum;
}
}
// ================= node vs rigid constraints ===================
btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
btReducedDeformableBody* rsb,
const btSoftBody::DeformableNodeRigidContact& contact,
const btContactSolverInfo& infoGlobal,
btScalar dt)
: m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
{
m_contactNormalA = contact.m_cti.m_normal;
m_contactNormalB = -contact.m_cti.m_normal;
if (contact.m_node->index < rsb->m_nodes.size())
{
m_nodeQueryIndex = contact.m_node->index;
}
else
{
m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
}
if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
m_relPosA = contact.m_c1;
}
else
{
m_relPosA = btVector3(0,0,0);
}
m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
if (m_collideStatic) // colliding with static object, only consider reduced deformable body's impulse factor
{
m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
}
else // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
{
m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
}
m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
m_tangentImpulseFactor = 0;
warmStarting();
}
void btReducedDeformableNodeRigidContactConstraint::warmStarting()
{
btVector3 va = getVa();
btVector3 vb = getVb();
m_bufferVelocityA = va;
m_bufferVelocityB = vb;
// we define the (+) direction of errors to be the outward surface normal of the rigid object
btVector3 v_rel = vb - va;
// get tangent direction of the relative velocity
btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
if (v_tangent.norm() < SIMD_EPSILON)
{
m_contactTangent = btVector3(0, 0, 0);
// tangent impulse factor
m_tangentImpulseFactor = 0;
m_tangentImpulseFactorInv = 0;
}
else
{
if (!m_collideMultibody)
{
m_contactTangent = v_tangent.normalized();
m_contactTangent2.setZero();
// tangent impulse factor 1
m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
// tangent impulse factor 2
m_tangentImpulseFactor2 = 0;
m_tangentImpulseFactorInv2 = 0;
}
else // multibody requires 2 contact directions
{
m_contactTangent = m_contact->t1;
m_contactTangent2 = m_contact->t2;
// tangent impulse factor 1
m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
// tangent impulse factor 2
m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
}
}
// initial guess for normal impulse
{
btScalar velocity_error = btDot(v_rel, m_contactNormalA); // magnitude of relative velocity
btScalar position_error = 0;
if (m_penetration > 0)
{
velocity_error += m_penetration / m_dt;
}
else
{
// add penetration correction vel
position_error = m_penetration * m_erp / m_dt;
}
// get the initial estimate of impulse magnitude to be applied
m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
}
// initial guess for tangential impulse
{
btScalar velocity_error = btDot(v_rel, m_contactTangent);
m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
if (m_collideMultibody)
{
btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
}
}
// warm starting
// applyImpulse(m_rhs * m_contactNormalA);
// if (!m_collideStatic)
// {
// const btSoftBody::sCti& cti = m_contact->m_cti;
// if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
// {
// m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
// }
// }
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
{
return m_node->m_v;
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
{
btVector3 deltaVa(0, 0, 0);
if (!m_collideStatic)
{
if (!m_collideMultibody) // for rigid body
{
deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
}
else // for multibody
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_n[k];
}
deltaVa = m_contact->m_cti.m_normal * vel;
// add in the tangential components of the va
vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_t1[k];
}
deltaVa += m_contact->t1 * vel;
vel = 0;
for (int k = 0; k < ndof; ++k)
{
vel += local_dv[k] * J_t2[k];
}
deltaVa += m_contact->t2 * vel;
}
}
}
return deltaVa;
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
{
// std::cout << "node: " << m_node->index << '\n';
return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
{
return m_node->m_splitv;
}
btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
return m_total_normal_dv + m_total_tangent_dv;
}
void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
// m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
// if (!m_collideStatic)
// {
// // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
// // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
// btVector3 v_after = getDeltaVb() + m_node->m_v;
// // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
// }
// std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';
}
// ================= face vs rigid constraints ===================
btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
btReducedDeformableBody* rsb,
const btSoftBody::DeformableFaceRigidContact& contact,
const btContactSolverInfo& infoGlobal,
btScalar dt,
bool useStrainLimiting)
: m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
{}
btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
return vb;
}
btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
return vb;
}
btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
if (m_face->m_n[0] == node)
{
return face_dv * contact->m_weights[0];
}
if (m_face->m_n[1] == node)
{
return face_dv * contact->m_weights[1];
}
btAssert(node == m_face->m_n[2]);
return face_dv * contact->m_weights[2];
}
void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
//
}

194
src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
View File

@ -1,194 +0,0 @@
#include "../btDeformableContactConstraint.h"
#include "btReducedDeformableBody.h"
// ================= static constraints ===================
class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
{
public:
btReducedDeformableBody* m_rsb;
btScalar m_dt;
btVector3 m_ri;
btVector3 m_targetPos;
btVector3 m_impulseDirection;
btMatrix3x3 m_impulseFactorMatrix;
btScalar m_impulseFactor;
btScalar m_rhs;
btScalar m_appliedImpulse;
btScalar m_erp;
btReducedDeformableStaticConstraint(btReducedDeformableBody* rsb,
btSoftBody::Node* node,
const btVector3& ri,
const btVector3& x0,
const btVector3& dir,
const btContactSolverInfo& infoGlobal,
btScalar dt);
// btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other);
btReducedDeformableStaticConstraint() {}
virtual ~btReducedDeformableStaticConstraint() {}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
// this calls reduced deformable body's applyFullSpaceImpulse
virtual void applyImpulse(const btVector3& impulse);
btVector3 getDeltaVa() const;
// virtual void applySplitImpulse(const btVector3& impulse) {}
};
// ================= base contact constraints ===================
class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint
{
public:
bool m_collideStatic; // flag for collision with static object
bool m_collideMultibody; // flag for collision with multibody
int m_nodeQueryIndex;
int m_solverBodyId; // for debugging
btReducedDeformableBody* m_rsb;
btSolverBody* m_solverBody;
btScalar m_dt;
btScalar m_appliedNormalImpulse;
btScalar m_appliedTangentImpulse;
btScalar m_appliedTangentImpulse2;
btScalar m_normalImpulseFactor;
btScalar m_tangentImpulseFactor;
btScalar m_tangentImpulseFactor2;
btScalar m_tangentImpulseFactorInv;
btScalar m_tangentImpulseFactorInv2;
btScalar m_rhs;
btScalar m_rhs_tangent;
btScalar m_rhs_tangent2;
btScalar m_cfm;
btScalar m_cfm_friction;
btScalar m_erp;
btScalar m_erp_friction;
btScalar m_friction;
btVector3 m_contactNormalA; // surface normal for rigid body (opposite direction as impulse)
btVector3 m_contactNormalB; // surface normal for reduced deformable body (opposite direction as impulse)
btVector3 m_contactTangent; // tangential direction of the relative velocity
btVector3 m_contactTangent2; // 2nd tangential direction of the relative velocity
btVector3 m_relPosA; // relative position of the contact point for A (rigid)
btVector3 m_relPosB; // relative position of the contact point for B
btMatrix3x3 m_impulseFactor; // total impulse matrix
btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration
btVector3 m_bufferVelocityB;
btVector3 m_linearComponentNormal; // linear components for the solver body
btVector3 m_angularComponentNormal; // angular components for the solver body
// since 2nd contact direction only applies to multibody, these components will never be used
btVector3 m_linearComponentTangent;
btVector3 m_angularComponentTangent;
btReducedDeformableRigidContactConstraint(btReducedDeformableBody* rsb,
const btSoftBody::DeformableRigidContact& c,
const btContactSolverInfo& infoGlobal,
btScalar dt);
// btReducedDeformableRigidContactConstraint(const btReducedDeformableRigidContactConstraint& other);
btReducedDeformableRigidContactConstraint() {}
virtual ~btReducedDeformableRigidContactConstraint() {}
void setSolverBody(const int bodyId, btSolverBody& solver_body);
virtual void warmStarting() {}
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
void calculateTangentialImpulse(btScalar& deltaImpulse_tangent,
btScalar& appliedImpulse,
const btScalar rhs_tangent,
const btScalar tangentImpulseFactorInv,
const btVector3& tangent,
const btScalar lower_limit,
const btScalar upper_limit,
const btVector3& deltaV_rel);
virtual void applyImpulse(const btVector3& impulse) {}
virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
virtual btVector3 getVa() const;
virtual btVector3 getDeltaVa() const = 0;
virtual btVector3 getDeltaVb() const = 0;
};
// ================= node vs rigid constraints ===================
class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformableRigidContactConstraint
{
public:
btSoftBody::Node* m_node;
btReducedDeformableNodeRigidContactConstraint(btReducedDeformableBody* rsb,
const btSoftBody::DeformableNodeRigidContact& contact,
const btContactSolverInfo& infoGlobal,
btScalar dt);
// btReducedDeformableNodeRigidContactConstraint(const btReducedDeformableNodeRigidContactConstraint& other);
btReducedDeformableNodeRigidContactConstraint() {}
virtual ~btReducedDeformableNodeRigidContactConstraint() {}
virtual void warmStarting();
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
// get the velocity change of the rigid body
virtual btVector3 getDeltaVa() const;
// get velocity change of the node in contat
virtual btVector3 getDeltaVb() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const;
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
// cast the contact to the desired type
const btSoftBody::DeformableNodeRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
}
// this calls reduced deformable body's applyFullSpaceImpulse
virtual void applyImpulse(const btVector3& impulse);
};
// ================= face vs rigid constraints ===================
class btReducedDeformableFaceRigidContactConstraint : public btReducedDeformableRigidContactConstraint
{
public:
btSoftBody::Face* m_face;
bool m_useStrainLimiting;
btReducedDeformableFaceRigidContactConstraint(btReducedDeformableBody* rsb,
const btSoftBody::DeformableFaceRigidContact& contact,
const btContactSolverInfo& infoGlobal,
btScalar dt,
bool useStrainLimiting);
// btReducedDeformableFaceRigidContactConstraint(const btReducedDeformableFaceRigidContactConstraint& other);
btReducedDeformableFaceRigidContactConstraint() {}
virtual ~btReducedDeformableFaceRigidContactConstraint() {}
// get the velocity of the deformable face at the contact point
virtual btVector3 getVb() const;
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const;
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
// cast the contact to the desired type
const btSoftBody::DeformableFaceRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
}
// this calls reduced deformable body's applyFullSpaceImpulse
virtual void applyImpulse(const btVector3& impulse);
};

10
src/BulletSoftBody/CMakeLists.txt
View File

@ -24,11 +24,6 @@ SET(BulletSoftBody_SRCS
btDeformableMultiBodyDynamicsWorld.cpp
btDeformableContactConstraint.cpp
poly34.cpp
BulletReducedDeformableBody/btReducedDeformableBody.cpp
BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp
BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp
)
@ -68,11 +63,6 @@ SET(BulletSoftBody_HDRS
poly34.h
btSoftBodySolverVertexBuffer.h
BulletReducedDeformableBody/btReducedDeformableBody.h
BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h
BulletReducedDeformableBody/btReducedDeformableBodySolver.h
BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
)

8
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -25,18 +25,12 @@
#include "btDeformableNeoHookeanForce.h"
#include "btDeformableContactProjection.h"
#include "btPreconditioner.h"
// #include "btDeformableMultiBodyDynamicsWorld.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "LinearMath/btQuickprof.h"
class btDeformableBackwardEulerObjective
{
public:
enum _
{
Mass_preconditioner,
KKT_preconditioner
};
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_dt;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;

89
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -23,7 +23,6 @@ btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
m_reducedSolver = false;
}
btDeformableBodySolver::~btDeformableBodySolver()
@ -402,7 +401,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
}
}
}
applyExplicitForce();
m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
@ -505,89 +504,3 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}
void btDeformableBodySolver::applyExplicitForce()
{
m_objective->applyExplicitForce(m_residual);
}
void btDeformableBodySolver::applyTransforms(btScalar timeStep)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
btScalar clampDeltaV = maxDisplacement / timeStep;
for (int c = 0; c < 3; c++)
{
if (node.m_v[c] > clampDeltaV)
{
node.m_v[c] = clampDeltaV;
}
if (node.m_v[c] < -clampDeltaV)
{
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
// enforce anchor constraints
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
// update multibody anchor info
if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 nrm;
const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
const btTransform& wtr = multibodyLinkCol->getWorldTransform();
psb->m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(n->m_x),
shp,
nrm,
0);
a.m_cti.m_normal = wtr.getBasis() * nrm;
btVector3 normal = a.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
a.jacobianData_normal = jacobianData_normal;
a.jacobianData_t1 = jacobianData_t1;
a.jacobianData_t2 = jacobianData_t2;
a.t1 = t1;
a.t2 = t2;
}
}
}
psb->interpolateRenderMesh();
}
}

74
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -24,8 +24,8 @@
#include "btConjugateResidual.h"
#include "btConjugateGradient.h"
struct btCollisionObjectWrapper;
// class btDeformableBackwardEulerObjective;
// class btDeformableMultiBodyDynamicsWorld;
class btDeformableBackwardEulerObjective;
class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
@ -46,7 +46,6 @@ protected:
int m_maxNewtonIterations; // max number of newton iterations
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
bool m_reducedSolver; // flag for reduced soft body solver
public:
// handles data related to objective function
btDeformableBackwardEulerObjective* m_objective;
@ -69,18 +68,11 @@ public:
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// set gravity (get from deformable world)
virtual void setGravity(const btVector3& gravity)
{
// for full deformable object, we don't store gravity in the solver
// this function is overriden in the reduced deformable object
}
// resize/clear data structures
virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
// set up contact constraints
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
void setConstraints(const btContactSolverInfo& infoGlobal);
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
virtual void predictMotion(btScalar solverdt);
@ -93,7 +85,7 @@ public:
void backupVelocity();
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
virtual void setupDeformableSolve(bool implicit);
void setupDeformableSolve(bool implicit);
// set the current velocity to that backed up in m_backupVelocity
void revertVelocity();
@ -158,62 +150,6 @@ public:
// used in line search
btScalar kineticEnergy();
// add explicit force to the velocity in the objective class
virtual void applyExplicitForce();
// execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world
virtual void applyTransforms(btScalar timeStep);
virtual void setStrainLimiting(bool opt)
{
m_objective->m_projection.m_useStrainLimiting = opt;
}
virtual void setPreconditioner(int opt)
{
switch (opt)
{
case btDeformableBackwardEulerObjective::Mass_preconditioner:
m_objective->m_preconditioner = m_objective->m_massPreconditioner;
break;
case btDeformableBackwardEulerObjective::KKT_preconditioner:
m_objective->m_preconditioner = m_objective->m_KKTPreconditioner;
break;
default:
btAssert(false);
break;
}
}
virtual btAlignedObjectArray<btDeformableLagrangianForce*>* getLagrangianForceArray()
{
return &(m_objective->m_lf);
}
virtual const btAlignedObjectArray<btSoftBody::Node*>* getIndices()
{
return m_objective->getIndices();
}
virtual void setProjection()
{
m_objective->m_projection.setProjection();
}
virtual void setLagrangeMultiplier()
{
m_objective->m_projection.setLagrangeMultiplier();
}
virtual bool isReducedSolver()
{
return m_reducedSolver;
}
virtual void deformableBodyInternalWriteBack() {}
// unused functions
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
virtual void solveConstraints(btScalar dt) {}

71
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -14,16 +14,11 @@
*/
#include "btDeformableMultiBodyConstraintSolver.h"
#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
#include <iostream>
#include <thread>
// override the iterations method to include deformable/multibody contact
btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
{
// pair deformable body with solver body
pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
@ -42,10 +37,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
// solver body velocity <- rigid body velocity
writeToSolverBody(bodies, numBodies, infoGlobal);
// std::cout << "------------Iteration " << iteration << "------------\n";
// std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
@ -60,9 +51,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
m_deformableSolver->deformableBodyInternalWriteBack();
// std::cout << "[===================Next Step===================]\n";
break;
}
}
@ -90,12 +78,6 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
// reduced soft body solver directly modifies the solver body
if (m_deformableSolver->isReducedSolver())
{
return;
}
for (int i = 0; i < numBodies; i++)
{
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
@ -112,12 +94,6 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject*
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
{
// reduced soft body solver directly modifies the solver body
if (m_deformableSolver->isReducedSolver())
{
return;
}
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
{
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
@ -129,53 +105,6 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
}
void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
if (!m_deformableSolver->isReducedSolver())
{
return;
}
btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
for (int i = 0; i < numDeformableBodies; ++i)
{
for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
{
btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
{
btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
// object index in the solver body pool
int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
if (body && body->getInvMass())
{
// std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
constraint.setSolverBody(bodyId, solverBody);
}
}
}
// for (int j = 0; j < numBodies; j++)
// {
// int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
// btRigidBody* body = btRigidBody::upcast(bodies[j]);
// if (body && body->getInvMass())
// {
// btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
// m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
// }
// }
}
}
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");

3
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@ -43,9 +43,6 @@ protected:
// write the velocity of the underlying rigid body to the the the solver body
void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
// let each deformable body knows which solver body is in constact
void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);

106
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
beforeSolverCallbacks(timeStep);
// ///solve contact constraints and then deformable bodies momemtum equation
///solve contact constraints and then deformable bodies momemtum equation
solveConstraints(timeStep);
afterSolverCallbacks(timeStep);
@ -298,7 +298,83 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
BT_PROFILE("integrateTransforms");
positionCorrection(timeStep);
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
m_deformableBodySolver->applyTransforms(timeStep);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
btScalar clampDeltaV = maxDisplacement / timeStep;
for (int c = 0; c < 3; c++)
{
if (node.m_v[c] > clampDeltaV)
{
node.m_v[c] = clampDeltaV;
}
if (node.m_v[c] < -clampDeltaV)
{
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
// enforce anchor constraints
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
// update multibody anchor info
if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 nrm;
const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
const btTransform& wtr = multibodyLinkCol->getWorldTransform();
psb->m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(n->m_x),
shp,
nrm,
0);
a.m_cti.m_normal = wtr.getBasis() * nrm;
btVector3 normal = a.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
a.jacobianData_normal = jacobianData_normal;
a.jacobianData_t1 = jacobianData_t1;
a.jacobianData_t2 = jacobianData_t2;
a.t1 = t1;
a.t2 = t2;
}
}
}
psb->interpolateRenderMesh();
}
}
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
@ -315,9 +391,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
// set up the directions in which the velocity does not change in the momentum solve
if (m_useProjection)
m_deformableBodySolver->setProjection();
m_deformableBodySolver->m_objective->m_projection.setProjection();
else
m_deformableBodySolver->setLagrangeMultiplier();
m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
// for explicit scheme, m_backupVelocity = v_{n+1}^*
// for implicit scheme, m_backupVelocity = v_n
@ -443,12 +519,6 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
m_deformableBodySolver->predictMotion(timeStep);
}
void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity)
{
btDiscreteDynamicsWorld::setGravity(gravity);
m_deformableBodySolver->setGravity(gravity);
}
void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
@ -463,14 +533,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
if (m_useProjection)
{
m_deformableBodySolver->m_useProjection = true;
m_deformableBodySolver->setStrainLimiting(true);
m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner);
m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
}
else
{
m_deformableBodySolver->m_useProjection = false;
m_deformableBodySolver->setStrainLimiting(false);
m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner);
m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
}
}
@ -612,7 +682,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
bool added = false;
for (int i = 0; i < forces.size(); ++i)
{
@ -626,14 +696,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
if (!added)
{
force->addSoftBody(psb);
force->setIndices(m_deformableBodySolver->getIndices());
force->setIndices(m_deformableBodySolver->m_objective->getIndices());
forces.push_back(force);
}
}
void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
int removed_index = -1;
for (int i = 0; i < forces.size(); ++i)
{
@ -651,7 +721,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab
void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
{
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
for (int i = 0; i < forces.size(); ++i)
{
forces[i]->removeSoftBody(psb);

4
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -19,7 +19,7 @@
#include "btSoftMultiBodyDynamicsWorld.h"
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
// #include "btDeformableBodySolver.h"
#include "btDeformableBodySolver.h"
#include "btDeformableMultiBodyConstraintSolver.h"
#include "btSoftBodyHelpers.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
@ -121,8 +121,6 @@ public:
return m_sbi;
}
virtual void setGravity(const btVector3& gravity);
void reinitialize(btScalar timeStep);
void applyRigidBodyGravity(btScalar timeStep);

3
src/BulletSoftBody/btSoftBody.cpp
View File

@ -231,9 +231,6 @@ void btSoftBody::initDefaults()
m_gravityFactor = 1;
m_cacheBarycenter = false;
m_fdbvnt = 0;
// reduced flag
m_reducedModel = false;
}
//

20
src/BulletSoftBody/btSoftBody.h
View File

@ -799,7 +799,6 @@ public:
typedef btAlignedObjectArray<Material*> tMaterialArray;
typedef btAlignedObjectArray<Joint*> tJointArray;
typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
//
// Fields
@ -858,8 +857,6 @@ public:
btScalar m_restLengthScale;
bool m_reducedModel; // Reduced deformable model flag
//
// Api
//
@ -1006,15 +1003,15 @@ public:
/* Get best fit rigid transform */
btTransform getRigidTransform();
/* Transform to given pose */
virtual void transformTo(const btTransform& trs);
void transformTo(const btTransform& trs);
/* Transform */
virtual void transform(const btTransform& trs);
void transform(const btTransform& trs);
/* Translate */
virtual void translate(const btVector3& trs);
void translate(const btVector3& trs);
/* Rotate */
virtual void rotate(const btQuaternion& rot);
void rotate(const btQuaternion& rot);
/* Scale */
virtual void scale(const btVector3& scl);
void scale(const btVector3& scl);
/* Get link resting lengths scale */
btScalar getRestLengthScale();
/* Scale resting length of all springs */
@ -1104,13 +1101,6 @@ public:
void setZeroVelocity();
bool wantsSleeping();
virtual btMatrix3x3 getImpulseFactor(int n_node)
{
btMatrix3x3 tmp;
tmp.setIdentity();
return tmp;
}
//
// Functionality to deal with new accelerated solvers.
//

26
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1691,19 +1691,12 @@ struct btSoftColliders
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
const btVector3 ra = n.m_x - wtr.getOrigin();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
if (psb->m_reducedModel)
{
c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse)
}
else
{
c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
// c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
}
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
// c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@ -1731,16 +1724,7 @@ struct btSoftColliders
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix;
if (psb->m_reducedModel)
{
local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof);
}
else
{
local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
}
btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;

3
src/BulletSoftBody/btSoftBodySolvers.h
View File

@ -36,8 +36,7 @@ public:
CL_SIMD_SOLVER,
DX_SOLVER,
DX_SIMD_SOLVER,
DEFORMABLE_SOLVER,
REDUCED_DEFORMABLE_SOLVER
DEFORMABLE_SOLVER
};
protected: