mirror of
https://github.com/godotengine/godot.git
synced 2024-11-26 22:23:04 +00:00
Merge pull request #44455 from akien-mga/bullet-3.07
bullet: Sync with upstream 3.07
This commit is contained in:
commit
8180b607b8
4
thirdparty/README.md
vendored
4
thirdparty/README.md
vendored
@ -40,11 +40,9 @@ Files extracted from upstream source:
|
||||
## bullet
|
||||
|
||||
- Upstream: https://github.com/bulletphysics/bullet3
|
||||
- Version: git pre-2.90 (cd8cf7521cbb8b7808126a6adebd47bb83ea166a, 2020)
|
||||
- Version: 3.07 (e32fc59c88a3908876949c6f2665e8d091d987fa, 2020)
|
||||
- License: zlib
|
||||
|
||||
Important: Synced with a pre-release version of bullet 2.90 from the master branch.
|
||||
|
||||
Files extracted from upstream source:
|
||||
|
||||
- src/* apart from CMakeLists.txt and premake4.lua files
|
||||
|
@ -285,7 +285,6 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int
|
||||
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart);
|
||||
|
||||
curNodeSubPart = nodeSubPart;
|
||||
b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
|
||||
}
|
||||
//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
|
||||
|
||||
@ -293,7 +292,13 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int
|
||||
|
||||
for (int j = 2; j >= 0; j--)
|
||||
{
|
||||
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
|
||||
int graphicsindex;
|
||||
switch (indicestype) {
|
||||
case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
|
||||
case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
|
||||
case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
|
||||
default: b3Assert(0);
|
||||
}
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
|
||||
|
@ -851,12 +851,12 @@ void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFla
|
||||
|
||||
void bFile::safeSwapPtr(char *dst, const char *src)
|
||||
{
|
||||
if (!src || !dst)
|
||||
return;
|
||||
|
||||
int ptrFile = mFileDNA->getPointerSize();
|
||||
int ptrMem = mMemoryDNA->getPointerSize();
|
||||
|
||||
if (!src && !dst)
|
||||
return;
|
||||
|
||||
if (ptrFile == ptrMem)
|
||||
{
|
||||
memcpy(dst, src, ptrMem);
|
||||
|
@ -346,8 +346,6 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
|
||||
}
|
||||
}
|
||||
|
||||
int maxIterations = 0;
|
||||
|
||||
void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const
|
||||
{
|
||||
btAssert(!m_useQuantization);
|
||||
@ -387,8 +385,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, cons
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
if (maxIterations < walkIterations)
|
||||
maxIterations = walkIterations;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -529,8 +525,6 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
if (maxIterations < walkIterations)
|
||||
maxIterations = walkIterations;
|
||||
}
|
||||
|
||||
void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const
|
||||
@ -654,8 +648,6 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
if (maxIterations < walkIterations)
|
||||
maxIterations = walkIterations;
|
||||
}
|
||||
|
||||
void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const
|
||||
@ -718,8 +710,6 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
if (maxIterations < walkIterations)
|
||||
maxIterations = walkIterations;
|
||||
}
|
||||
|
||||
//This traversal can be called from Playstation 3 SPU
|
||||
|
@ -127,6 +127,7 @@ public:
|
||||
|
||||
enum CollisionFlags
|
||||
{
|
||||
CF_DYNAMIC_OBJECT = 0,
|
||||
CF_STATIC_OBJECT = 1,
|
||||
CF_KINEMATIC_OBJECT = 2,
|
||||
CF_NO_CONTACT_RESPONSE = 4,
|
||||
@ -251,6 +252,16 @@ public:
|
||||
m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
|
||||
}
|
||||
|
||||
int getNumObjectsWithoutCollision() const
|
||||
{
|
||||
return m_objectsWithoutCollisionCheck.size();
|
||||
}
|
||||
|
||||
const btCollisionObject* getObjectWithoutCollision(int index)
|
||||
{
|
||||
return m_objectsWithoutCollisionCheck[index];
|
||||
}
|
||||
|
||||
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
|
||||
{
|
||||
int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
|
||||
|
@ -361,7 +361,13 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle
|
||||
|
||||
for (int j = 2; j >= 0; j--)
|
||||
{
|
||||
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
|
||||
int graphicsindex;
|
||||
switch (indicestype) {
|
||||
case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
|
||||
case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
|
||||
case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
|
||||
default: btAssert(0);
|
||||
}
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
|
||||
|
@ -124,12 +124,17 @@ void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const
|
||||
nodeSubPart);
|
||||
|
||||
unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
|
||||
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
|
||||
|
||||
const btVector3& meshScaling = m_meshInterface->getScaling();
|
||||
for (int j = 2; j >= 0; j--)
|
||||
{
|
||||
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
|
||||
int graphicsindex;
|
||||
switch (indicestype) {
|
||||
case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
|
||||
case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
|
||||
case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
|
||||
default: btAssert(0);
|
||||
}
|
||||
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
@ -193,12 +198,17 @@ void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, con
|
||||
nodeSubPart);
|
||||
|
||||
unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
|
||||
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
|
||||
|
||||
const btVector3& meshScaling = m_meshInterface->getScaling();
|
||||
for (int j = 2; j >= 0; j--)
|
||||
{
|
||||
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
|
||||
int graphicsindex;
|
||||
switch (indicestype) {
|
||||
case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
|
||||
case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
|
||||
case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
|
||||
default: btAssert(0);
|
||||
}
|
||||
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
|
@ -30,11 +30,12 @@ protected:
|
||||
int m_shapeType;
|
||||
void* m_userPointer;
|
||||
int m_userIndex;
|
||||
int m_userIndex2;
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
|
||||
btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1), m_userIndex2(-1)
|
||||
{
|
||||
}
|
||||
|
||||
@ -137,6 +138,16 @@ public:
|
||||
return m_userIndex;
|
||||
}
|
||||
|
||||
void setUserIndex2(int index)
|
||||
{
|
||||
m_userIndex2 = index;
|
||||
}
|
||||
|
||||
int getUserIndex2() const
|
||||
{
|
||||
return m_userIndex2;
|
||||
}
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
|
@ -21,8 +21,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
|
||||
int heightStickWidth, int heightStickLength, const void* heightfieldData,
|
||||
btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
|
||||
PHY_ScalarType hdt, bool flipQuadEdges)
|
||||
:m_userIndex2(-1),
|
||||
m_userValue3(0),
|
||||
:m_userValue3(0),
|
||||
m_triangleInfoMap(0)
|
||||
{
|
||||
initialize(heightStickWidth, heightStickLength, heightfieldData,
|
||||
@ -31,8 +30,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
|
||||
}
|
||||
|
||||
btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
|
||||
:m_userIndex2(-1),
|
||||
m_userValue3(0),
|
||||
: m_userValue3(0),
|
||||
m_triangleInfoMap(0)
|
||||
{
|
||||
// legacy constructor: support only float or unsigned char,
|
||||
|
@ -114,7 +114,7 @@ protected:
|
||||
int m_vboundsGridLength;
|
||||
int m_vboundsChunkSize;
|
||||
|
||||
int m_userIndex2;
|
||||
|
||||
btScalar m_userValue3;
|
||||
|
||||
struct btTriangleInfoMap* m_triangleInfoMap;
|
||||
@ -192,14 +192,6 @@ public:
|
||||
virtual const char* getName() const { return "HEIGHTFIELD"; }
|
||||
|
||||
|
||||
void setUserIndex2(int index)
|
||||
{
|
||||
m_userIndex2 = index;
|
||||
}
|
||||
int getUserIndex2() const
|
||||
{
|
||||
return m_userIndex2;
|
||||
}
|
||||
void setUserValue3(btScalar value)
|
||||
{
|
||||
m_userValue3 = value;
|
||||
|
@ -286,7 +286,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int
|
||||
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart);
|
||||
|
||||
curNodeSubPart = nodeSubPart;
|
||||
btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
|
||||
}
|
||||
//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
|
||||
|
||||
@ -294,7 +293,13 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int
|
||||
|
||||
for (int j = 2; j >= 0; j--)
|
||||
{
|
||||
int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
|
||||
int graphicsindex;
|
||||
switch (indicestype) {
|
||||
case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
|
||||
case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
|
||||
case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
|
||||
default: btAssert(0);
|
||||
}
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
|
||||
|
@ -2,8 +2,11 @@
|
||||
#include "btMiniSDF.h"
|
||||
#include "LinearMath/btAabbUtil2.h"
|
||||
|
||||
struct btSdfCollisionShapeInternalData
|
||||
ATTRIBUTE_ALIGNED16(struct)
|
||||
btSdfCollisionShapeInternalData
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_localScaling;
|
||||
btScalar m_margin;
|
||||
btMiniSDF m_sdf;
|
||||
|
@ -623,13 +623,21 @@ public:
|
||||
i1 = s_indices[1];
|
||||
i2 = s_indices[2];
|
||||
}
|
||||
else
|
||||
else if (indicestype == PHY_INTEGER)
|
||||
{
|
||||
unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride);
|
||||
i0 = i_indices[0];
|
||||
i1 = i_indices[1];
|
||||
i2 = i_indices[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(indicestype == PHY_UCHAR);
|
||||
unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride);
|
||||
i0 = i_indices[0];
|
||||
i1 = i_indices[1];
|
||||
i2 = i_indices[2];
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const
|
||||
|
@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
|
||||
const btScalar length = delta.length();
|
||||
results.normal = delta / length;
|
||||
results.witnesses[0] += results.normal * margin;
|
||||
return (length - margin);
|
||||
results.distance = length - margin;
|
||||
return results.distance;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
@ -852,7 +852,7 @@ static void setupSpatialGridBatchesMt(
|
||||
memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows);
|
||||
size_t scratchSize = memHelper.getSizeToAllocate();
|
||||
// if we need to reallocate
|
||||
if (scratchMemory->capacity() < scratchSize)
|
||||
if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize)
|
||||
{
|
||||
// allocate 6.25% extra to avoid repeated reallocs
|
||||
scratchMemory->reserve(scratchSize + scratchSize / 16);
|
||||
|
@ -47,6 +47,8 @@ struct btContactSolverInfoData
|
||||
btScalar m_erp; //error reduction for non-contact constraints
|
||||
btScalar m_erp2; //error reduction for contact constraints
|
||||
btScalar m_deformable_erp; //error reduction for deformable constraints
|
||||
btScalar m_deformable_cfm; //constraint force mixing for deformable constraints
|
||||
btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact
|
||||
btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts
|
||||
btScalar m_frictionERP; //error reduction for friction constraints
|
||||
btScalar m_frictionCFM; //constraint force mixing for friction constraints
|
||||
@ -83,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.2);
|
||||
m_erp2 = btScalar(0.2);
|
||||
m_deformable_erp = btScalar(0.1);
|
||||
m_deformable_erp = btScalar(0.06);
|
||||
m_deformable_cfm = btScalar(0.01);
|
||||
m_deformable_maxErrorReduction = btScalar(0.1);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
|
||||
m_frictionCFM = btScalar(0.);
|
||||
|
@ -356,12 +356,12 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 getPushVelocity()
|
||||
btVector3 getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
btVector3 getTurnVelocity()
|
||||
btVector3 getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
@ -465,6 +465,12 @@ public:
|
||||
//for kinematic objects, we could also use use:
|
||||
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
|
||||
}
|
||||
|
||||
btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
|
||||
{
|
||||
//we also calculate lin/ang velocity for kinematic objects
|
||||
return m_pushVelocity + m_turnVelocity.cross(rel_pos);
|
||||
}
|
||||
|
||||
void translate(const btVector3& v)
|
||||
{
|
||||
|
@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof()
|
||||
{
|
||||
m_deltaV.resize(0);
|
||||
m_deltaV.resize(6 + m_dofCount);
|
||||
m_splitV.resize(0);
|
||||
m_splitV.resize(6 + m_dofCount);
|
||||
m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
|
||||
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
|
||||
m_matrixBuf.resize(m_links.size() + 1);
|
||||
@ -671,6 +673,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i)
|
||||
return &m_links[i].m_jointTorque[0];
|
||||
}
|
||||
|
||||
bool btMultiBody::hasFixedBase() const
|
||||
{
|
||||
return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject());
|
||||
}
|
||||
|
||||
bool btMultiBody::isBaseStaticOrKinematic() const
|
||||
{
|
||||
return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject());
|
||||
}
|
||||
|
||||
bool btMultiBody::isBaseKinematic() const
|
||||
{
|
||||
return getBaseCollider() && getBaseCollider()->isKinematicObject();
|
||||
}
|
||||
|
||||
void btMultiBody::setBaseDynamicType(int dynamicType)
|
||||
{
|
||||
if(getBaseCollider()) {
|
||||
int oldFlags = getBaseCollider()->getCollisionFlags();
|
||||
oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
|
||||
getBaseCollider()->setCollisionFlags(oldFlags | dynamicType);
|
||||
}
|
||||
}
|
||||
|
||||
inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
|
||||
{
|
||||
btVector3 row0 = btVector3(
|
||||
@ -796,7 +822,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
|
||||
spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
|
||||
|
||||
if (m_fixedBase)
|
||||
if (isBaseStaticOrKinematic())
|
||||
{
|
||||
zeroAccSpatFrc[0].setZero();
|
||||
}
|
||||
@ -872,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
|
||||
// calculate zhat_i^A
|
||||
//
|
||||
//external forces
|
||||
btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
|
||||
btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
|
||||
if (isLinkAndAllAncestorsKinematic(i))
|
||||
{
|
||||
zeroAccSpatFrc[i].setZero();
|
||||
}
|
||||
else{
|
||||
//external forces
|
||||
btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
|
||||
btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
|
||||
|
||||
zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
|
||||
zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
|
||||
|
||||
#if 0
|
||||
{
|
||||
{
|
||||
|
||||
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
|
||||
i+1,
|
||||
zeroAccSpatFrc[i+1].m_topVec[0],
|
||||
zeroAccSpatFrc[i+1].m_topVec[1],
|
||||
zeroAccSpatFrc[i+1].m_topVec[2],
|
||||
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
|
||||
i+1,
|
||||
zeroAccSpatFrc[i+1].m_topVec[0],
|
||||
zeroAccSpatFrc[i+1].m_topVec[1],
|
||||
zeroAccSpatFrc[i+1].m_topVec[2],
|
||||
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[0],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[1],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[2]);
|
||||
}
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[0],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[1],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[2]);
|
||||
}
|
||||
#endif
|
||||
//
|
||||
//adding damping terms (only)
|
||||
btScalar linDampMult = 1., angDampMult = 1.;
|
||||
zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
|
||||
linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
|
||||
//
|
||||
//adding damping terms (only)
|
||||
btScalar linDampMult = 1., angDampMult = 1.;
|
||||
zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
|
||||
linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
|
||||
//p += vhat x Ihat vhat - done in a simpler way
|
||||
if (m_useGyroTerm)
|
||||
zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
|
||||
//
|
||||
zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
|
||||
//
|
||||
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
|
||||
////clamp parent's omega
|
||||
//btScalar parOmegaMod = temp.length();
|
||||
//btScalar parOmegaModMax = 1000;
|
||||
//if(parOmegaMod > parOmegaModMax)
|
||||
// temp *= parOmegaModMax / parOmegaMod;
|
||||
//zeroAccSpatFrc[i+1].addLinear(temp);
|
||||
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
|
||||
//temp = spatCoriolisAcc[i].getLinear();
|
||||
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
|
||||
}
|
||||
|
||||
// calculate Ihat_i^A
|
||||
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
|
||||
@ -909,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
|
||||
0, m_links[i].m_inertiaLocal[1], 0,
|
||||
0, 0, m_links[i].m_inertiaLocal[2]));
|
||||
//
|
||||
//p += vhat x Ihat vhat - done in a simpler way
|
||||
if (m_useGyroTerm)
|
||||
zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
|
||||
//
|
||||
zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
|
||||
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
|
||||
////clamp parent's omega
|
||||
//btScalar parOmegaMod = temp.length();
|
||||
//btScalar parOmegaModMax = 1000;
|
||||
//if(parOmegaMod > parOmegaModMax)
|
||||
// temp *= parOmegaModMax / parOmegaMod;
|
||||
//zeroAccSpatFrc[i+1].addLinear(temp);
|
||||
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
|
||||
//temp = spatCoriolisAcc[i].getLinear();
|
||||
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
|
||||
|
||||
//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
|
||||
//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
|
||||
@ -935,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
// (part of TreeForwardDynamics in Mirtich.)
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
if(isLinkAndAllAncestorsKinematic(i))
|
||||
continue;
|
||||
const int parent = m_links[i].m_parent;
|
||||
fromParent.m_rotMat = rot_from_parent[i + 1];
|
||||
fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
||||
@ -1047,7 +1081,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
// Second 'upward' loop
|
||||
// (part of TreeForwardDynamics in Mirtich)
|
||||
|
||||
if (m_fixedBase)
|
||||
if (isBaseStaticOrKinematic())
|
||||
{
|
||||
spatAcc[0].setZero();
|
||||
}
|
||||
@ -1081,22 +1115,24 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
||||
|
||||
fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
|
||||
|
||||
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
if(!isLinkAndAllAncestorsKinematic(i))
|
||||
{
|
||||
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
||||
//
|
||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
|
||||
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
||||
//
|
||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
|
||||
}
|
||||
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
|
||||
//D^{-1} * (Y - h^{T}*apar)
|
||||
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
||||
|
||||
spatAcc[i + 1] += spatCoriolisAcc[i];
|
||||
|
||||
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
||||
}
|
||||
|
||||
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
|
||||
//D^{-1} * (Y - h^{T}*apar)
|
||||
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
||||
|
||||
spatAcc[i + 1] += spatCoriolisAcc[i];
|
||||
|
||||
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
||||
|
||||
if (m_links[i].m_jointFeedback)
|
||||
{
|
||||
m_internalNeedsJointFeedback = true;
|
||||
@ -1432,7 +1468,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
|
||||
// Fill in zero_acc
|
||||
// -- set to force/torque on the base, zero otherwise
|
||||
if (m_fixedBase)
|
||||
if (isBaseStaticOrKinematic())
|
||||
{
|
||||
zeroAccSpatFrc[0].setZero();
|
||||
}
|
||||
@ -1451,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
// (part of TreeForwardDynamics in Mirtich.)
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
if(isLinkAndAllAncestorsKinematic(i))
|
||||
continue;
|
||||
const int parent = m_links[i].m_parent;
|
||||
fromParent.m_rotMat = rot_from_parent[i + 1];
|
||||
fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
||||
@ -1494,7 +1532,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
// Second 'upward' loop
|
||||
// (part of TreeForwardDynamics in Mirtich)
|
||||
|
||||
if (m_fixedBase)
|
||||
if (isBaseStaticOrKinematic())
|
||||
{
|
||||
spatAcc[0].setZero();
|
||||
}
|
||||
@ -1507,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
// now do the loop over the m_links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
if(isLinkAndAllAncestorsKinematic(i))
|
||||
continue;
|
||||
const int parent = m_links[i].m_parent;
|
||||
fromParent.m_rotMat = rot_from_parent[i + 1];
|
||||
fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
||||
@ -1550,23 +1590,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
void btMultiBody::predictPositionsMultiDof(btScalar dt)
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos;
|
||||
btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
if(!isBaseKinematic())
|
||||
{
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos;
|
||||
btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
|
||||
// reset to current position
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
m_basePos_interpolate[i] = m_basePos[i];
|
||||
}
|
||||
pBasePos = m_basePos_interpolate;
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
// reset to current position
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
m_basePos_interpolate[i] = m_basePos[i];
|
||||
}
|
||||
pBasePos = m_basePos_interpolate;
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
}
|
||||
|
||||
///////////////////////////////
|
||||
//local functor for quaternion integration (to avoid error prone redundancy)
|
||||
@ -1617,26 +1660,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
|
||||
|
||||
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
||||
//
|
||||
btScalar *pBaseQuat;
|
||||
if(!isBaseKinematic())
|
||||
{
|
||||
btScalar *pBaseQuat;
|
||||
|
||||
// reset to current orientation
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
m_baseQuat_interpolate[i] = m_baseQuat[i];
|
||||
}
|
||||
pBaseQuat = m_baseQuat_interpolate;
|
||||
// reset to current orientation
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
m_baseQuat_interpolate[i] = m_baseQuat[i];
|
||||
}
|
||||
pBaseQuat = m_baseQuat_interpolate;
|
||||
|
||||
btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
||||
btVector3 baseOmega;
|
||||
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
||||
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
||||
pBaseQuat[0] = baseQuat.x();
|
||||
pBaseQuat[1] = baseQuat.y();
|
||||
pBaseQuat[2] = baseQuat.z();
|
||||
pBaseQuat[3] = baseQuat.w();
|
||||
btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
||||
btVector3 baseOmega;
|
||||
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
||||
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
||||
pBaseQuat[0] = baseQuat.x();
|
||||
pBaseQuat[1] = baseQuat.y();
|
||||
pBaseQuat[2] = baseQuat.z();
|
||||
pBaseQuat[3] = baseQuat.w();
|
||||
}
|
||||
|
||||
// Finally we can update m_jointPos for each of the m_links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
@ -1644,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
|
||||
btScalar *pJointPos;
|
||||
pJointPos = &m_links[i].m_jointPos_interpolate[0];
|
||||
|
||||
btScalar *pJointVel = getJointVelMultiDof(i);
|
||||
|
||||
switch (m_links[i].m_jointType)
|
||||
if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic())
|
||||
{
|
||||
switch (m_links[i].m_jointType)
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
pJointPos[0] = m_links[i].m_jointPos[0];
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
for (int j = 0; j < 3; ++j)
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
pJointPos[0] = m_links[i].m_jointPos[0];
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
btScalar *pJointVel = getJointVelMultiDof(i);
|
||||
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
||||
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
||||
pJointPos[0] = jointOri.x();
|
||||
pJointPos[1] = jointOri.y();
|
||||
pJointPos[2] = jointOri.z();
|
||||
pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
switch (m_links[i].m_jointType)
|
||||
{
|
||||
for (int j = 0; j < 3; ++j)
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
pJointPos[0] = m_links[i].m_jointPos[0];
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
||||
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
||||
pJointPos[0] = jointOri.x();
|
||||
pJointPos[1] = jointOri.y();
|
||||
pJointPos[2] = jointOri.z();
|
||||
pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
for (int j = 0; j < 3; ++j)
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
||||
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
||||
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
pJointPos[j] = m_links[i].m_jointPos[j];
|
||||
}
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
||||
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
||||
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
@ -1703,16 +1782,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
|
||||
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
|
||||
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
if(!isBaseKinematic())
|
||||
{
|
||||
// step position by adding dt * velocity
|
||||
//btVector3 v = getBaseVel();
|
||||
//m_basePos += dt * v;
|
||||
//
|
||||
btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
|
||||
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
||||
|
||||
pBasePos[0] += dt * pBaseVel[0];
|
||||
pBasePos[1] += dt * pBaseVel[1];
|
||||
pBasePos[2] += dt * pBaseVel[2];
|
||||
}
|
||||
|
||||
///////////////////////////////
|
||||
//local functor for quaternion integration (to avoid error prone redundancy)
|
||||
@ -1763,22 +1845,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
|
||||
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
||||
//
|
||||
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
|
||||
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
||||
btVector3 baseOmega;
|
||||
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
||||
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
||||
pBaseQuat[0] = baseQuat.x();
|
||||
pBaseQuat[1] = baseQuat.y();
|
||||
pBaseQuat[2] = baseQuat.z();
|
||||
pBaseQuat[3] = baseQuat.w();
|
||||
if(!isBaseKinematic())
|
||||
{
|
||||
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
|
||||
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
||||
//
|
||||
btQuaternion baseQuat;
|
||||
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
||||
btVector3 baseOmega;
|
||||
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
||||
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
||||
pBaseQuat[0] = baseQuat.x();
|
||||
pBaseQuat[1] = baseQuat.y();
|
||||
pBaseQuat[2] = baseQuat.z();
|
||||
pBaseQuat[3] = baseQuat.w();
|
||||
|
||||
//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
|
||||
//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
|
||||
//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
|
||||
//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
|
||||
//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
|
||||
//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
|
||||
}
|
||||
|
||||
if (pq)
|
||||
pq += 7;
|
||||
@ -1788,48 +1873,51 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
// Finally we can update m_jointPos for each of the m_links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
btScalar *pJointPos;
|
||||
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
|
||||
|
||||
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
|
||||
|
||||
switch (m_links[i].m_jointType)
|
||||
if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()))
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
||||
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
||||
pJointPos[0] = jointOri.x();
|
||||
pJointPos[1] = jointOri.y();
|
||||
pJointPos[2] = jointOri.z();
|
||||
pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
btScalar *pJointPos;
|
||||
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
|
||||
|
||||
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
||||
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
||||
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
switch (m_links[i].m_jointType)
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
//reset to current pos
|
||||
btScalar jointVel = pJointVel[0];
|
||||
pJointPos[0] += dt * jointVel;
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
{
|
||||
//reset to current pos
|
||||
btVector3 jointVel;
|
||||
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
||||
btQuaternion jointOri;
|
||||
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
||||
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
||||
pJointPos[0] = jointOri.x();
|
||||
pJointPos[1] = jointOri.y();
|
||||
pJointPos[2] = jointOri.z();
|
||||
pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
|
||||
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
||||
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
||||
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
||||
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -2135,8 +2223,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj
|
||||
world_to_local.resize(getNumLinks() + 1);
|
||||
local_origin.resize(getNumLinks() + 1);
|
||||
|
||||
world_to_local[0] = getInterpolateWorldToBaseRot();
|
||||
local_origin[0] = getInterpolateBasePos();
|
||||
if(isBaseKinematic()){
|
||||
world_to_local[0] = getWorldToBaseRot();
|
||||
local_origin[0] = getBasePos();
|
||||
}
|
||||
else
|
||||
{
|
||||
world_to_local[0] = getInterpolateWorldToBaseRot();
|
||||
local_origin[0] = getInterpolateBasePos();
|
||||
}
|
||||
|
||||
if (getBaseCollider())
|
||||
{
|
||||
@ -2282,3 +2377,81 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali
|
||||
|
||||
return btMultiBodyDataName;
|
||||
}
|
||||
|
||||
void btMultiBody::saveKinematicState(btScalar timeStep)
|
||||
{
|
||||
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
|
||||
if (timeStep != btScalar(0.))
|
||||
{
|
||||
btVector3 linearVelocity, angularVelocity;
|
||||
btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity);
|
||||
setBaseVel(linearVelocity);
|
||||
setBaseOmega(angularVelocity);
|
||||
setInterpolateBaseWorldTransform(getBaseWorldTransform());
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::setLinkDynamicType(const int i, int type)
|
||||
{
|
||||
if (i == -1)
|
||||
{
|
||||
setBaseDynamicType(type);
|
||||
}
|
||||
else if (i >= 0 && i < getNumLinks())
|
||||
{
|
||||
if (m_links[i].m_collider)
|
||||
{
|
||||
m_links[i].m_collider->setDynamicType(type);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool btMultiBody::isLinkStaticOrKinematic(const int i) const
|
||||
{
|
||||
if (i == -1)
|
||||
{
|
||||
return isBaseStaticOrKinematic();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_links[i].m_collider)
|
||||
return m_links[i].m_collider->isStaticOrKinematic();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool btMultiBody::isLinkKinematic(const int i) const
|
||||
{
|
||||
if (i == -1)
|
||||
{
|
||||
return isBaseKinematic();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_links[i].m_collider)
|
||||
return m_links[i].m_collider->isKinematic();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const
|
||||
{
|
||||
int link = i;
|
||||
while (link != -1) {
|
||||
if (!isLinkStaticOrKinematic(link))
|
||||
return false;
|
||||
link = m_links[link].m_parent;
|
||||
}
|
||||
return isBaseStaticOrKinematic();
|
||||
}
|
||||
|
||||
bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const
|
||||
{
|
||||
int link = i;
|
||||
while (link != -1) {
|
||||
if (!isLinkKinematic(link))
|
||||
return false;
|
||||
link = m_links[link].m_parent;
|
||||
}
|
||||
return isBaseKinematic();
|
||||
}
|
||||
|
@ -210,7 +210,13 @@ public:
|
||||
void setBasePos(const btVector3 &pos)
|
||||
{
|
||||
m_basePos = pos;
|
||||
m_basePos_interpolate = pos;
|
||||
if(!isBaseKinematic())
|
||||
m_basePos_interpolate = pos;
|
||||
}
|
||||
|
||||
void setInterpolateBasePos(const btVector3 &pos)
|
||||
{
|
||||
m_basePos_interpolate = pos;
|
||||
}
|
||||
|
||||
void setBaseWorldTransform(const btTransform &tr)
|
||||
@ -227,17 +233,39 @@ public:
|
||||
return tr;
|
||||
}
|
||||
|
||||
void setInterpolateBaseWorldTransform(const btTransform &tr)
|
||||
{
|
||||
setInterpolateBasePos(tr.getOrigin());
|
||||
setInterpolateWorldToBaseRot(tr.getRotation().inverse());
|
||||
}
|
||||
|
||||
btTransform getInterpolateBaseWorldTransform() const
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setOrigin(getInterpolateBasePos());
|
||||
tr.setRotation(getInterpolateWorldToBaseRot().inverse());
|
||||
return tr;
|
||||
}
|
||||
|
||||
void setBaseVel(const btVector3 &vel)
|
||||
{
|
||||
m_realBuf[3] = vel[0];
|
||||
m_realBuf[4] = vel[1];
|
||||
m_realBuf[5] = vel[2];
|
||||
}
|
||||
|
||||
void setWorldToBaseRot(const btQuaternion &rot)
|
||||
{
|
||||
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
|
||||
m_baseQuat_interpolate = rot;
|
||||
if(!isBaseKinematic())
|
||||
m_baseQuat_interpolate = rot;
|
||||
}
|
||||
|
||||
void setInterpolateWorldToBaseRot(const btQuaternion &rot)
|
||||
{
|
||||
m_baseQuat_interpolate = rot;
|
||||
}
|
||||
|
||||
void setBaseOmega(const btVector3 &omega)
|
||||
{
|
||||
m_realBuf[0] = omega[0];
|
||||
@ -245,6 +273,8 @@ public:
|
||||
m_realBuf[2] = omega[2];
|
||||
}
|
||||
|
||||
void saveKinematicState(btScalar timeStep);
|
||||
|
||||
//
|
||||
// get/set pos/vel for child m_links (i = 0 to num_links-1)
|
||||
//
|
||||
@ -278,6 +308,11 @@ public:
|
||||
{
|
||||
return &m_deltaV[0];
|
||||
}
|
||||
|
||||
const btScalar *getSplitVelocityVector() const
|
||||
{
|
||||
return &m_splitV[0];
|
||||
}
|
||||
/* btScalar * getVelocityVector()
|
||||
{
|
||||
return &real_buf[0];
|
||||
@ -397,6 +432,26 @@ public:
|
||||
m_deltaV[dof] += delta_vee[dof] * multiplier;
|
||||
}
|
||||
}
|
||||
void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
|
||||
{
|
||||
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
|
||||
{
|
||||
m_splitV[dof] += delta_vee[dof] * multiplier;
|
||||
}
|
||||
}
|
||||
void addSplitV()
|
||||
{
|
||||
applyDeltaVeeMultiDof(&m_splitV[0], 1);
|
||||
}
|
||||
void substractSplitV()
|
||||
{
|
||||
applyDeltaVeeMultiDof(&m_splitV[0], -1);
|
||||
|
||||
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
|
||||
{
|
||||
m_splitV[dof] = 0.f;
|
||||
}
|
||||
}
|
||||
void processDeltaVeeMultiDof2()
|
||||
{
|
||||
applyDeltaVeeMultiDof(&m_deltaV[0], 1);
|
||||
@ -495,14 +550,22 @@ public:
|
||||
void goToSleep();
|
||||
void checkMotionAndSleepIfRequired(btScalar timestep);
|
||||
|
||||
bool hasFixedBase() const
|
||||
{
|
||||
return m_fixedBase;
|
||||
}
|
||||
bool hasFixedBase() const;
|
||||
|
||||
bool isBaseKinematic() const;
|
||||
|
||||
bool isBaseStaticOrKinematic() const;
|
||||
|
||||
// set the dynamic type in the base's collision flags.
|
||||
void setBaseDynamicType(int dynamicType);
|
||||
|
||||
void setFixedBase(bool fixedBase)
|
||||
{
|
||||
m_fixedBase = fixedBase;
|
||||
if(m_fixedBase)
|
||||
setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT);
|
||||
else
|
||||
setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
|
||||
}
|
||||
|
||||
int getCompanionId() const
|
||||
@ -653,7 +716,15 @@ public:
|
||||
btVector3 &top_out, // top part of output vector
|
||||
btVector3 &bottom_out); // bottom part of output vector
|
||||
|
||||
void setLinkDynamicType(const int i, int type);
|
||||
|
||||
bool isLinkStaticOrKinematic(const int i) const;
|
||||
|
||||
bool isLinkKinematic(const int i) const;
|
||||
|
||||
bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
|
||||
|
||||
bool isLinkAndAllAncestorsKinematic(const int i) const;
|
||||
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
@ -711,6 +782,7 @@ private:
|
||||
// offset size array
|
||||
// 0 num_links+1 rot_from_parent
|
||||
//
|
||||
btAlignedObjectArray<btScalar> m_splitV;
|
||||
btAlignedObjectArray<btScalar> m_deltaV;
|
||||
btAlignedObjectArray<btScalar> m_realBuf;
|
||||
btAlignedObjectArray<btVector3> m_vectorBuf;
|
||||
|
@ -2,11 +2,12 @@
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
|
||||
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
|
||||
: m_bodyA(bodyA),
|
||||
m_bodyB(bodyB),
|
||||
m_linkA(linkA),
|
||||
m_linkB(linkB),
|
||||
m_type(type),
|
||||
m_numRows(numRows),
|
||||
m_jacSizeA(0),
|
||||
m_jacSizeBoth(0),
|
||||
|
@ -20,6 +20,21 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "btMultiBody.h"
|
||||
|
||||
|
||||
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
|
||||
enum btTypedMultiBodyConstraintType
|
||||
{
|
||||
MULTIBODY_CONSTRAINT_LIMIT=3,
|
||||
MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
|
||||
MULTIBODY_CONSTRAINT_GEAR,
|
||||
MULTIBODY_CONSTRAINT_POINT_TO_POINT,
|
||||
MULTIBODY_CONSTRAINT_SLIDER,
|
||||
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
|
||||
MULTIBODY_CONSTRAINT_FIXED,
|
||||
|
||||
MAX_MULTIBODY_CONSTRAINT_TYPE,
|
||||
};
|
||||
|
||||
class btMultiBody;
|
||||
struct btSolverInfo;
|
||||
|
||||
@ -46,6 +61,8 @@ protected:
|
||||
int m_linkA;
|
||||
int m_linkB;
|
||||
|
||||
int m_type; //btTypedMultiBodyConstraintType
|
||||
|
||||
int m_numRows;
|
||||
int m_jacSizeA;
|
||||
int m_jacSizeBoth;
|
||||
@ -82,12 +99,16 @@ protected:
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral);
|
||||
btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
|
||||
virtual ~btMultiBodyConstraint();
|
||||
|
||||
void updateJacobianSizes();
|
||||
void allocateJacobiansMultiDof();
|
||||
|
||||
int getConstraintType() const
|
||||
{
|
||||
return m_type;
|
||||
}
|
||||
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
|
||||
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
|
||||
virtual void setPivotInB(const btVector3& pivotInB) {}
|
||||
|
@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
|
||||
|
||||
if (!isSleeping)
|
||||
{
|
||||
bod->addSplitV();
|
||||
int nLinks = bod->getNumLinks();
|
||||
|
||||
///base + num m_links
|
||||
@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
|
||||
m_scratch_world_to_local.resize(nLinks + 1);
|
||||
m_scratch_local_origin.resize(nLinks + 1);
|
||||
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
|
||||
bod->substractSplitV();
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -867,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep)
|
||||
{
|
||||
btDiscreteDynamicsWorld::saveKinematicState(timeStep);
|
||||
for(int i = 0; i < m_multiBodies.size(); i++)
|
||||
{
|
||||
btMultiBody* body = m_multiBodies[i];
|
||||
if(body->isBaseKinematic())
|
||||
body->saveKinematicState(timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
|
||||
//{
|
||||
|
@ -120,5 +120,7 @@ public:
|
||||
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
|
||||
virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
|
||||
void buildIslands();
|
||||
|
||||
virtual void saveKinematicState(btScalar timeStep);
|
||||
};
|
||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
@ -24,7 +24,7 @@ subject to the following restrictions:
|
||||
#define BTMBFIXEDCONSTRAINT_DIM 6
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li
|
||||
}
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
|
@ -21,7 +21,7 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
|
||||
m_gearRatio(1),
|
||||
m_gearAuxLink(-1),
|
||||
m_erp(0),
|
||||
|
@ -22,7 +22,7 @@ subject to the following restrictions:
|
||||
|
||||
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,2,true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
|
||||
m_lowerBound(lower),
|
||||
m_upperBound(upper)
|
||||
{
|
||||
|
@ -42,6 +42,22 @@ public:
|
||||
{
|
||||
//todo(erwincoumans)
|
||||
}
|
||||
btScalar getLowerBound() const
|
||||
{
|
||||
return m_lowerBound;
|
||||
}
|
||||
btScalar getUpperBound() const
|
||||
{
|
||||
return m_upperBound;
|
||||
}
|
||||
void setLowerBound(btScalar lower)
|
||||
{
|
||||
m_lowerBound = lower;
|
||||
}
|
||||
void setUpperBound(btScalar upper)
|
||||
{
|
||||
m_upperBound = upper;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
@ -21,7 +21,7 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
|
@ -295,6 +295,9 @@ struct btMultibodyLink
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_LINK_H
|
||||
|
@ -130,6 +130,23 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isStaticOrKinematic() const
|
||||
{
|
||||
return isStaticOrKinematicObject();
|
||||
}
|
||||
|
||||
bool isKinematic() const
|
||||
{
|
||||
return isKinematicObject();
|
||||
}
|
||||
|
||||
void setDynamicType(int dynamicType)
|
||||
{
|
||||
int oldFlags = getCollisionFlags();
|
||||
oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
|
||||
setCollisionFlags(oldFlags | dynamicType);
|
||||
}
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
|
@ -27,7 +27,7 @@ subject to the following restrictions:
|
||||
#endif
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
|
@ -25,7 +25,7 @@ subject to the following restrictions:
|
||||
#define EPSILON 0.000001
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int
|
||||
}
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
|
@ -23,7 +23,7 @@ subject to the following restrictions:
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
|
||||
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
|
||||
m_desiredVelocity(0, 0, 0),
|
||||
m_desiredPosition(0,0,0,1),
|
||||
m_kd(1.),
|
||||
|
@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
|
||||
btDeformableMultiBodyConstraintSolver* m_deformableSolver;
|
||||
|
||||
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
|
||||
btDispatcher* dispatcher)
|
||||
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
|
||||
btDispatcher* dispatcher)
|
||||
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
virtual void processConstraints(int islandId=-1)
|
||||
virtual void processConstraints(int islandId = -1)
|
||||
{
|
||||
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
|
||||
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
|
||||
@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
|
||||
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
|
||||
|
||||
m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
|
||||
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
|
||||
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1))
|
||||
{
|
||||
m_deformableSolver->m_analyticsData.m_islandId = islandId;
|
||||
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
|
||||
|
144
thirdparty/bullet/BulletSoftBody/btCGProjection.h
vendored
144
thirdparty/bullet/BulletSoftBody/btCGProjection.h
vendored
@ -22,85 +22,83 @@
|
||||
|
||||
struct DeformableContactConstraint
|
||||
{
|
||||
const btSoftBody::Node* m_node;
|
||||
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
|
||||
btAlignedObjectArray<btVector3> m_total_normal_dv;
|
||||
btAlignedObjectArray<btVector3> m_total_tangent_dv;
|
||||
btAlignedObjectArray<bool> m_static;
|
||||
btAlignedObjectArray<bool> m_can_be_dynamic;
|
||||
|
||||
DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
|
||||
{
|
||||
append(rcontact);
|
||||
}
|
||||
|
||||
DeformableContactConstraint(): m_node(NULL)
|
||||
{
|
||||
m_contact.push_back(NULL);
|
||||
}
|
||||
|
||||
void append(const btSoftBody::RContact& rcontact)
|
||||
{
|
||||
m_contact.push_back(&rcontact);
|
||||
m_total_normal_dv.push_back(btVector3(0,0,0));
|
||||
m_total_tangent_dv.push_back(btVector3(0,0,0));
|
||||
m_static.push_back(false);
|
||||
m_can_be_dynamic.push_back(true);
|
||||
}
|
||||
const btSoftBody::Node* m_node;
|
||||
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
|
||||
btAlignedObjectArray<btVector3> m_total_normal_dv;
|
||||
btAlignedObjectArray<btVector3> m_total_tangent_dv;
|
||||
btAlignedObjectArray<bool> m_static;
|
||||
btAlignedObjectArray<bool> m_can_be_dynamic;
|
||||
|
||||
void replace(const btSoftBody::RContact& rcontact)
|
||||
{
|
||||
m_contact.clear();
|
||||
m_total_normal_dv.clear();
|
||||
m_total_tangent_dv.clear();
|
||||
m_static.clear();
|
||||
m_can_be_dynamic.clear();
|
||||
append(rcontact);
|
||||
}
|
||||
|
||||
~DeformableContactConstraint()
|
||||
{
|
||||
}
|
||||
DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node)
|
||||
{
|
||||
append(rcontact);
|
||||
}
|
||||
|
||||
DeformableContactConstraint() : m_node(NULL)
|
||||
{
|
||||
m_contact.push_back(NULL);
|
||||
}
|
||||
|
||||
void append(const btSoftBody::RContact& rcontact)
|
||||
{
|
||||
m_contact.push_back(&rcontact);
|
||||
m_total_normal_dv.push_back(btVector3(0, 0, 0));
|
||||
m_total_tangent_dv.push_back(btVector3(0, 0, 0));
|
||||
m_static.push_back(false);
|
||||
m_can_be_dynamic.push_back(true);
|
||||
}
|
||||
|
||||
void replace(const btSoftBody::RContact& rcontact)
|
||||
{
|
||||
m_contact.clear();
|
||||
m_total_normal_dv.clear();
|
||||
m_total_tangent_dv.clear();
|
||||
m_static.clear();
|
||||
m_can_be_dynamic.clear();
|
||||
append(rcontact);
|
||||
}
|
||||
|
||||
~DeformableContactConstraint()
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class btCGProjection
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
|
||||
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
const btScalar& m_dt;
|
||||
// map from node indices to node pointers
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
|
||||
|
||||
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
|
||||
: m_softBodies(softBodies)
|
||||
, m_dt(dt)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btCGProjection()
|
||||
{
|
||||
}
|
||||
|
||||
// apply the constraints
|
||||
virtual void project(TVStack& x) = 0;
|
||||
|
||||
virtual void setConstraints() = 0;
|
||||
|
||||
// update the constraints
|
||||
virtual btScalar update() = 0;
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
|
||||
{
|
||||
m_nodes = nodes;
|
||||
}
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
|
||||
btAlignedObjectArray<btSoftBody*>& m_softBodies;
|
||||
const btScalar& m_dt;
|
||||
// map from node indices to node pointers
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
|
||||
|
||||
btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
|
||||
: m_softBodies(softBodies), m_dt(dt)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btCGProjection()
|
||||
{
|
||||
}
|
||||
|
||||
// apply the constraints
|
||||
virtual void project(TVStack& x) = 0;
|
||||
|
||||
virtual void setConstraints() = 0;
|
||||
|
||||
// update the constraints
|
||||
virtual btScalar update() = 0;
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
|
||||
{
|
||||
m_nodes = nodes;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif /* btCGProjection_h */
|
||||
|
@ -15,144 +15,103 @@
|
||||
|
||||
#ifndef BT_CONJUGATE_GRADIENT_H
|
||||
#define BT_CONJUGATE_GRADIENT_H
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <LinearMath/btAlignedObjectArray.h>
|
||||
#include <LinearMath/btVector3.h>
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btKrylovSolver.h"
|
||||
template <class MatrixX>
|
||||
class btConjugateGradient
|
||||
class btConjugateGradient : public btKrylovSolver<MatrixX>
|
||||
{
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
TVStack r,p,z,temp;
|
||||
int max_iterations;
|
||||
btScalar tolerance_squared;
|
||||
public:
|
||||
btConjugateGradient(const int max_it_in)
|
||||
: max_iterations(max_it_in)
|
||||
{
|
||||
tolerance_squared = 1e-5;
|
||||
}
|
||||
|
||||
virtual ~btConjugateGradient(){}
|
||||
|
||||
// return the number of iterations taken
|
||||
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
|
||||
{
|
||||
BT_PROFILE("CGSolve");
|
||||
btAssert(x.size() == b.size());
|
||||
reinitialize(b);
|
||||
// r = b - A * x --with assigned dof zeroed out
|
||||
A.multiply(x, temp);
|
||||
r = sub(b, temp);
|
||||
A.project(r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z);
|
||||
A.project(z);
|
||||
btScalar r_dot_z = dot(z,r);
|
||||
if (r_dot_z <= tolerance_squared) {
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Iteration = 0" << std::endl;
|
||||
std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
p = z;
|
||||
btScalar r_dot_z_new = r_dot_z;
|
||||
for (int k = 1; k <= max_iterations; k++) {
|
||||
// temp = A*p
|
||||
A.multiply(p, temp);
|
||||
A.project(temp);
|
||||
if (dot(p,temp) < SIMD_EPSILON)
|
||||
{
|
||||
if (verbose)
|
||||
std::cout << "Encountered negative direction in CG!" << std::endl;
|
||||
if (k == 1)
|
||||
{
|
||||
x = b;
|
||||
}
|
||||
return k;
|
||||
}
|
||||
// alpha = r^T * z / (p^T * A * p)
|
||||
btScalar alpha = r_dot_z_new / dot(p, temp);
|
||||
// x += alpha * p;
|
||||
multAndAddTo(alpha, p, x);
|
||||
// r -= alpha * temp;
|
||||
multAndAddTo(-alpha, temp, r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z);
|
||||
r_dot_z = r_dot_z_new;
|
||||
r_dot_z_new = dot(r,z);
|
||||
if (r_dot_z_new < tolerance_squared) {
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateGradient iterations " << k << std::endl;
|
||||
}
|
||||
return k;
|
||||
}
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btKrylovSolver<MatrixX> Base;
|
||||
TVStack r, p, z, temp;
|
||||
|
||||
btScalar beta = r_dot_z_new/r_dot_z;
|
||||
p = multAndAdd(beta, p, z);
|
||||
}
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
|
||||
}
|
||||
return max_iterations;
|
||||
}
|
||||
|
||||
void reinitialize(const TVStack& b)
|
||||
{
|
||||
r.resize(b.size());
|
||||
p.resize(b.size());
|
||||
z.resize(b.size());
|
||||
temp.resize(b.size());
|
||||
}
|
||||
|
||||
TVStack sub(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// c = a-b
|
||||
btAssert(a.size() == b.size());
|
||||
TVStack c;
|
||||
c.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
{
|
||||
c[i] = a[i] - b[i];
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
btScalar squaredNorm(const TVStack& a)
|
||||
{
|
||||
return dot(a,a);
|
||||
}
|
||||
|
||||
btScalar dot(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
btScalar ans(0);
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
ans += a[i].dot(b[i]);
|
||||
return ans;
|
||||
}
|
||||
|
||||
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
|
||||
{
|
||||
// result += s*a
|
||||
btAssert(a.size() == result.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] += s * a[i];
|
||||
}
|
||||
|
||||
TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// result = a*s + b
|
||||
TVStack result;
|
||||
result.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] = s * a[i] + b[i];
|
||||
return result;
|
||||
}
|
||||
public:
|
||||
btConjugateGradient(const int max_it_in)
|
||||
: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btConjugateGradient() {}
|
||||
|
||||
// return the number of iterations taken
|
||||
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
|
||||
{
|
||||
BT_PROFILE("CGSolve");
|
||||
btAssert(x.size() == b.size());
|
||||
reinitialize(b);
|
||||
temp = b;
|
||||
A.project(temp);
|
||||
p = temp;
|
||||
A.precondition(p, z);
|
||||
btScalar d0 = this->dot(z, temp);
|
||||
d0 = btMin(btScalar(1), d0);
|
||||
// r = b - A * x --with assigned dof zeroed out
|
||||
A.multiply(x, temp);
|
||||
r = this->sub(b, temp);
|
||||
A.project(r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z);
|
||||
A.project(z);
|
||||
btScalar r_dot_z = this->dot(z, r);
|
||||
if (r_dot_z <= Base::m_tolerance * d0)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Iteration = 0" << std::endl;
|
||||
std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
p = z;
|
||||
btScalar r_dot_z_new = r_dot_z;
|
||||
for (int k = 1; k <= Base::m_maxIterations; k++)
|
||||
{
|
||||
// temp = A*p
|
||||
A.multiply(p, temp);
|
||||
A.project(temp);
|
||||
if (this->dot(p, temp) < 0)
|
||||
{
|
||||
if (verbose)
|
||||
std::cout << "Encountered negative direction in CG!" << std::endl;
|
||||
if (k == 1)
|
||||
{
|
||||
x = b;
|
||||
}
|
||||
return k;
|
||||
}
|
||||
// alpha = r^T * z / (p^T * A * p)
|
||||
btScalar alpha = r_dot_z_new / this->dot(p, temp);
|
||||
// x += alpha * p;
|
||||
this->multAndAddTo(alpha, p, x);
|
||||
// r -= alpha * temp;
|
||||
this->multAndAddTo(-alpha, temp, r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z);
|
||||
r_dot_z = r_dot_z_new;
|
||||
r_dot_z_new = this->dot(r, z);
|
||||
if (r_dot_z_new < Base::m_tolerance * d0)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl;
|
||||
}
|
||||
return k;
|
||||
}
|
||||
|
||||
btScalar beta = r_dot_z_new / r_dot_z;
|
||||
p = this->multAndAdd(beta, p, z);
|
||||
}
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl;
|
||||
}
|
||||
return Base::m_maxIterations;
|
||||
}
|
||||
|
||||
void reinitialize(const TVStack& b)
|
||||
{
|
||||
r.resize(b.size());
|
||||
p.resize(b.size());
|
||||
z.resize(b.size());
|
||||
temp.resize(b.size());
|
||||
}
|
||||
};
|
||||
#endif /* btConjugateGradient_h */
|
||||
|
@ -15,174 +15,98 @@
|
||||
|
||||
#ifndef BT_CONJUGATE_RESIDUAL_H
|
||||
#define BT_CONJUGATE_RESIDUAL_H
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <LinearMath/btAlignedObjectArray.h>
|
||||
#include <LinearMath/btVector3.h>
|
||||
#include <LinearMath/btScalar.h>
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btKrylovSolver.h"
|
||||
|
||||
template <class MatrixX>
|
||||
class btConjugateResidual
|
||||
class btConjugateResidual : public btKrylovSolver<MatrixX>
|
||||
{
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
TVStack r,p,z,temp_p, temp_r, best_x;
|
||||
// temp_r = A*r
|
||||
// temp_p = A*p
|
||||
// z = M^(-1) * temp_p = M^(-1) * A * p
|
||||
int max_iterations;
|
||||
btScalar tolerance_squared, best_r;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btKrylovSolver<MatrixX> Base;
|
||||
TVStack r, p, z, temp_p, temp_r, best_x;
|
||||
// temp_r = A*r
|
||||
// temp_p = A*p
|
||||
// z = M^(-1) * temp_p = M^(-1) * A * p
|
||||
btScalar best_r;
|
||||
|
||||
public:
|
||||
btConjugateResidual(const int max_it_in)
|
||||
: max_iterations(max_it_in)
|
||||
{
|
||||
tolerance_squared = 1e-2;
|
||||
}
|
||||
|
||||
virtual ~btConjugateResidual(){}
|
||||
|
||||
// return the number of iterations taken
|
||||
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
|
||||
{
|
||||
BT_PROFILE("CRSolve");
|
||||
btAssert(x.size() == b.size());
|
||||
reinitialize(b);
|
||||
// r = b - A * x --with assigned dof zeroed out
|
||||
A.multiply(x, temp_r); // borrow temp_r here to store A*x
|
||||
r = sub(b, temp_r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z); // borrow z to store preconditioned r
|
||||
r = z;
|
||||
btScalar residual_norm = norm(r);
|
||||
if (residual_norm <= tolerance_squared) {
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Iteration = 0" << std::endl;
|
||||
std::cout << "Two norm of the residual = " << residual_norm << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
p = r;
|
||||
btScalar r_dot_Ar, r_dot_Ar_new;
|
||||
// temp_p = A*p
|
||||
A.multiply(p, temp_p);
|
||||
// temp_r = A*r
|
||||
temp_r = temp_p;
|
||||
r_dot_Ar = dot(r, temp_r);
|
||||
for (int k = 1; k <= max_iterations; k++) {
|
||||
// z = M^(-1) * Ap
|
||||
A.precondition(temp_p, z);
|
||||
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
|
||||
btScalar alpha = r_dot_Ar / dot(temp_p, z);
|
||||
// x += alpha * p;
|
||||
multAndAddTo(alpha, p, x);
|
||||
// r -= alpha * z;
|
||||
multAndAddTo(-alpha, z, r);
|
||||
btScalar norm_r = norm(r);
|
||||
if (norm_r < best_r)
|
||||
{
|
||||
best_x = x;
|
||||
best_r = norm_r;
|
||||
if (norm_r < tolerance_squared) {
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateResidual iterations " << k << std::endl;
|
||||
}
|
||||
return k;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
// temp_r = A * r;
|
||||
A.multiply(r, temp_r);
|
||||
r_dot_Ar_new = dot(r, temp_r);
|
||||
btScalar beta = r_dot_Ar_new/r_dot_Ar;
|
||||
r_dot_Ar = r_dot_Ar_new;
|
||||
// p = beta*p + r;
|
||||
p = multAndAdd(beta, p, r);
|
||||
// temp_p = beta*temp_p + temp_r;
|
||||
temp_p = multAndAdd(beta, temp_p, temp_r);
|
||||
}
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
|
||||
}
|
||||
x = best_x;
|
||||
return max_iterations;
|
||||
}
|
||||
|
||||
void reinitialize(const TVStack& b)
|
||||
{
|
||||
r.resize(b.size());
|
||||
p.resize(b.size());
|
||||
z.resize(b.size());
|
||||
temp_p.resize(b.size());
|
||||
temp_r.resize(b.size());
|
||||
best_x.resize(b.size());
|
||||
best_r = SIMD_INFINITY;
|
||||
}
|
||||
|
||||
TVStack sub(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// c = a-b
|
||||
btAssert(a.size() == b.size());
|
||||
TVStack c;
|
||||
c.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
{
|
||||
c[i] = a[i] - b[i];
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
btScalar squaredNorm(const TVStack& a)
|
||||
{
|
||||
return dot(a,a);
|
||||
}
|
||||
|
||||
btScalar norm(const TVStack& a)
|
||||
{
|
||||
btScalar ret = 0;
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
ret = btMax(ret, btFabs(a[i][d]));
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
btScalar dot(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
btScalar ans(0);
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
ans += a[i].dot(b[i]);
|
||||
return ans;
|
||||
}
|
||||
|
||||
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
|
||||
{
|
||||
// result += s*a
|
||||
btAssert(a.size() == result.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] += s * a[i];
|
||||
}
|
||||
|
||||
TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// result = a*s + b
|
||||
TVStack result;
|
||||
result.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] = s * a[i] + b[i];
|
||||
return result;
|
||||
}
|
||||
btConjugateResidual(const int max_it_in)
|
||||
: Base(max_it_in, 1e-8)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btConjugateResidual() {}
|
||||
|
||||
// return the number of iterations taken
|
||||
int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
|
||||
{
|
||||
BT_PROFILE("CRSolve");
|
||||
btAssert(x.size() == b.size());
|
||||
reinitialize(b);
|
||||
// r = b - A * x --with assigned dof zeroed out
|
||||
A.multiply(x, temp_r); // borrow temp_r here to store A*x
|
||||
r = this->sub(b, temp_r);
|
||||
// z = M^(-1) * r
|
||||
A.precondition(r, z); // borrow z to store preconditioned r
|
||||
r = z;
|
||||
btScalar residual_norm = this->norm(r);
|
||||
if (residual_norm <= Base::m_tolerance)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
p = r;
|
||||
btScalar r_dot_Ar, r_dot_Ar_new;
|
||||
// temp_p = A*p
|
||||
A.multiply(p, temp_p);
|
||||
// temp_r = A*r
|
||||
temp_r = temp_p;
|
||||
r_dot_Ar = this->dot(r, temp_r);
|
||||
for (int k = 1; k <= Base::m_maxIterations; k++)
|
||||
{
|
||||
// z = M^(-1) * Ap
|
||||
A.precondition(temp_p, z);
|
||||
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
|
||||
btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
|
||||
// x += alpha * p;
|
||||
this->multAndAddTo(alpha, p, x);
|
||||
// r -= alpha * z;
|
||||
this->multAndAddTo(-alpha, z, r);
|
||||
btScalar norm_r = this->norm(r);
|
||||
if (norm_r < best_r)
|
||||
{
|
||||
best_x = x;
|
||||
best_r = norm_r;
|
||||
if (norm_r < Base::m_tolerance)
|
||||
{
|
||||
return k;
|
||||
}
|
||||
}
|
||||
// temp_r = A * r;
|
||||
A.multiply(r, temp_r);
|
||||
r_dot_Ar_new = this->dot(r, temp_r);
|
||||
btScalar beta = r_dot_Ar_new / r_dot_Ar;
|
||||
r_dot_Ar = r_dot_Ar_new;
|
||||
// p = beta*p + r;
|
||||
p = this->multAndAdd(beta, p, r);
|
||||
// temp_p = beta*temp_p + temp_r;
|
||||
temp_p = this->multAndAdd(beta, temp_p, temp_r);
|
||||
}
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
|
||||
}
|
||||
x = best_x;
|
||||
return Base::m_maxIterations;
|
||||
}
|
||||
|
||||
void reinitialize(const TVStack& b)
|
||||
{
|
||||
r.resize(b.size());
|
||||
p.resize(b.size());
|
||||
z.resize(b.size());
|
||||
temp_p.resize(b.size());
|
||||
temp_r.resize(b.size());
|
||||
best_x.resize(b.size());
|
||||
best_r = SIMD_INFINITY;
|
||||
}
|
||||
};
|
||||
#endif /* btConjugateResidual_h */
|
||||
|
||||
|
@ -17,211 +17,283 @@
|
||||
#include "btPreconditioner.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
|
||||
: m_softBodies(softBodies)
|
||||
, m_projection(softBodies)
|
||||
, m_backupVelocity(backup_v)
|
||||
, m_implicit(false)
|
||||
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
|
||||
: m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
|
||||
{
|
||||
m_massPreconditioner = new MassPreconditioner(m_softBodies);
|
||||
m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
|
||||
m_preconditioner = m_KKTPreconditioner;
|
||||
m_massPreconditioner = new MassPreconditioner(m_softBodies);
|
||||
m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
|
||||
m_preconditioner = m_KKTPreconditioner;
|
||||
}
|
||||
|
||||
btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
|
||||
{
|
||||
delete m_KKTPreconditioner;
|
||||
delete m_massPreconditioner;
|
||||
delete m_KKTPreconditioner;
|
||||
delete m_massPreconditioner;
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
|
||||
{
|
||||
BT_PROFILE("reinitialize");
|
||||
if (dt > 0)
|
||||
{
|
||||
setDt(dt);
|
||||
}
|
||||
if(nodeUpdated)
|
||||
{
|
||||
updateId();
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
m_lf[i]->reinitialize(nodeUpdated);
|
||||
}
|
||||
m_projection.reinitialize(nodeUpdated);
|
||||
// m_preconditioner->reinitialize(nodeUpdated);
|
||||
BT_PROFILE("reinitialize");
|
||||
if (dt > 0)
|
||||
{
|
||||
setDt(dt);
|
||||
}
|
||||
if (nodeUpdated)
|
||||
{
|
||||
updateId();
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
m_lf[i]->reinitialize(nodeUpdated);
|
||||
}
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (psb->m_nodes[j].m_im > 0)
|
||||
psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im);
|
||||
}
|
||||
}
|
||||
m_projection.reinitialize(nodeUpdated);
|
||||
// m_preconditioner->reinitialize(nodeUpdated);
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::setDt(btScalar dt)
|
||||
{
|
||||
m_dt = dt;
|
||||
m_dt = dt;
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
|
||||
{
|
||||
BT_PROFILE("multiply");
|
||||
// add in the mass term
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
BT_PROFILE("multiply");
|
||||
// add in the mass term
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im;
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// add damping matrix
|
||||
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
|
||||
if (m_implicit)
|
||||
{
|
||||
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
|
||||
}
|
||||
}
|
||||
int offset = m_nodes.size();
|
||||
for (int i = offset; i < b.size(); ++i)
|
||||
{
|
||||
b[i].setZero();
|
||||
}
|
||||
// add in the lagrange multiplier terms
|
||||
|
||||
for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
// C * x
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// add damping matrix
|
||||
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
|
||||
// Always integrate picking force implicitly for stability.
|
||||
if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
|
||||
{
|
||||
m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b);
|
||||
}
|
||||
}
|
||||
int offset = m_nodes.size();
|
||||
for (int i = offset; i < b.size(); ++i)
|
||||
{
|
||||
b[i].setZero();
|
||||
}
|
||||
// add in the lagrange multiplier terms
|
||||
|
||||
for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
// C * x
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& node = psb->m_nodes[j];
|
||||
node.m_v = m_backupVelocity[node.index] + dv[node.index];
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& node = psb->m_nodes[j];
|
||||
node.m_v = m_backupVelocity[node.index] + dv[node.index];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
|
||||
{
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
|
||||
psb->m_nodes[j].m_v += one_over_mass * force[counter++];
|
||||
}
|
||||
}
|
||||
if (setZero)
|
||||
{
|
||||
for (int i = 0; i < force.size(); ++i)
|
||||
force[i].setZero();
|
||||
}
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
if (m_implicit)
|
||||
{
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (psb->m_nodes[j].m_im != 0)
|
||||
{
|
||||
psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
|
||||
psb->m_nodes[j].m_v += one_over_mass * force[counter++];
|
||||
}
|
||||
}
|
||||
}
|
||||
if (setZero)
|
||||
{
|
||||
for (int i = 0; i < force.size(); ++i)
|
||||
force[i].setZero();
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual)
|
||||
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual)
|
||||
{
|
||||
BT_PROFILE("computeResidual");
|
||||
// add implicit force
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
if (m_implicit)
|
||||
{
|
||||
m_lf[i]->addScaledForces(dt, residual);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lf[i]->addScaledDampingForce(dt, residual);
|
||||
}
|
||||
}
|
||||
// m_projection.project(residual);
|
||||
BT_PROFILE("computeResidual");
|
||||
// add implicit force
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// Always integrate picking force implicitly for stability.
|
||||
if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
|
||||
{
|
||||
m_lf[i]->addScaledForces(dt, residual);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lf[i]->addScaledDampingForce(dt, residual);
|
||||
}
|
||||
}
|
||||
// m_projection.project(residual);
|
||||
}
|
||||
|
||||
btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
|
||||
{
|
||||
btScalar mag = 0;
|
||||
for (int i = 0; i < residual.size(); ++i)
|
||||
{
|
||||
mag += residual[i].length2();
|
||||
}
|
||||
return std::sqrt(mag);
|
||||
btScalar mag = 0;
|
||||
for (int i = 0; i < residual.size(); ++i)
|
||||
{
|
||||
mag += residual[i].length2();
|
||||
}
|
||||
return std::sqrt(mag);
|
||||
}
|
||||
|
||||
btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
|
||||
{
|
||||
btScalar e = 0;
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
e += m_lf[i]->totalEnergy(dt);
|
||||
}
|
||||
return e;
|
||||
btScalar e = 0;
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
e += m_lf[i]->totalEnergy(dt);
|
||||
}
|
||||
return e;
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
m_softBodies[i]->advanceDeformation();
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
m_lf[i]->addScaledExplicitForce(m_dt, force);
|
||||
}
|
||||
applyForce(force, true);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
m_softBodies[i]->advanceDeformation();
|
||||
}
|
||||
if (m_implicit)
|
||||
{
|
||||
// apply forces except gravity force
|
||||
btVector3 gravity;
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE)
|
||||
{
|
||||
gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lf[i]->addScaledForces(m_dt, force);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
m_lf[i]->addScaledHessian(m_dt);
|
||||
}
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
// add gravity explicitly
|
||||
psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
m_lf[i]->addScaledExplicitForce(m_dt, force);
|
||||
}
|
||||
}
|
||||
// calculate inverse mass matrix for all nodes
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (psb->m_nodes[j].m_im > 0)
|
||||
{
|
||||
psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
applyForce(force, true);
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
|
||||
{
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
dv[counter] = psb->m_nodes[j].m_im * residual[counter];
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
dv[counter] = psb->m_nodes[j].m_im * residual[counter];
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//set constraints as projections
|
||||
void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
m_projection.setConstraints(infoGlobal);
|
||||
m_projection.setConstraints(infoGlobal);
|
||||
}
|
||||
|
||||
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
|
||||
{
|
||||
m_projection.applyDynamicFriction(r);
|
||||
m_projection.applyDynamicFriction(r);
|
||||
}
|
||||
|
@ -31,143 +31,168 @@
|
||||
class btDeformableBackwardEulerObjective
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_dt;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
|
||||
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
Preconditioner* m_preconditioner;
|
||||
btDeformableContactProjection m_projection;
|
||||
const TVStack& m_backupVelocity;
|
||||
btAlignedObjectArray<btSoftBody::Node* > m_nodes;
|
||||
bool m_implicit;
|
||||
MassPreconditioner* m_massPreconditioner;
|
||||
KKTPreconditioner* m_KKTPreconditioner;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_dt;
|
||||
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
|
||||
btAlignedObjectArray<btSoftBody*>& m_softBodies;
|
||||
Preconditioner* m_preconditioner;
|
||||
btDeformableContactProjection m_projection;
|
||||
const TVStack& m_backupVelocity;
|
||||
btAlignedObjectArray<btSoftBody::Node*> m_nodes;
|
||||
bool m_implicit;
|
||||
MassPreconditioner* m_massPreconditioner;
|
||||
KKTPreconditioner* m_KKTPreconditioner;
|
||||
|
||||
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
|
||||
|
||||
virtual ~btDeformableBackwardEulerObjective();
|
||||
|
||||
void initialize(){}
|
||||
|
||||
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
|
||||
void computeResidual(btScalar dt, TVStack& residual);
|
||||
|
||||
// add explicit force to the velocity
|
||||
void applyExplicitForce(TVStack& force);
|
||||
|
||||
// apply force to velocity and optionally reset the force to zero
|
||||
void applyForce(TVStack& force, bool setZero);
|
||||
|
||||
// compute the norm of the residual
|
||||
btScalar computeNorm(const TVStack& residual) const;
|
||||
|
||||
// compute one step of the solve (there is only one solve if the system is linear)
|
||||
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
|
||||
|
||||
// perform A*x = b
|
||||
void multiply(const TVStack& x, TVStack& b) const;
|
||||
|
||||
// set initial guess for CG solve
|
||||
void initialGuess(TVStack& dv, const TVStack& residual);
|
||||
|
||||
// reset data structure and reset dt
|
||||
void reinitialize(bool nodeUpdated, btScalar dt);
|
||||
|
||||
void setDt(btScalar dt);
|
||||
|
||||
// add friction force to residual
|
||||
void applyDynamicFriction(TVStack& r);
|
||||
|
||||
// add dv to velocity
|
||||
void updateVelocity(const TVStack& dv);
|
||||
|
||||
//set constraints as projections
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// update the projections and project the residual
|
||||
void project(TVStack& r)
|
||||
{
|
||||
BT_PROFILE("project");
|
||||
m_projection.project(r);
|
||||
}
|
||||
|
||||
// perform precondition M^(-1) x = b
|
||||
void precondition(const TVStack& x, TVStack& b)
|
||||
{
|
||||
m_preconditioner->operator()(x,b);
|
||||
}
|
||||
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
|
||||
|
||||
// reindex all the vertices
|
||||
virtual void updateId()
|
||||
{
|
||||
size_t node_id = 0;
|
||||
size_t face_id = 0;
|
||||
m_nodes.clear();
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].index = node_id;
|
||||
m_nodes.push_back(&psb->m_nodes[j]);
|
||||
++node_id;
|
||||
}
|
||||
for (int j = 0; j < psb->m_faces.size(); ++j)
|
||||
{
|
||||
psb->m_faces[j].m_index = face_id;
|
||||
++face_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
|
||||
{
|
||||
return &m_nodes;
|
||||
}
|
||||
|
||||
void setImplicit(bool implicit)
|
||||
{
|
||||
m_implicit = implicit;
|
||||
}
|
||||
virtual ~btDeformableBackwardEulerObjective();
|
||||
|
||||
// Calculate the total potential energy in the system
|
||||
btScalar totalEnergy(btScalar dt);
|
||||
|
||||
void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
|
||||
{
|
||||
extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
|
||||
for (int i = 0; i < vec.size(); ++i)
|
||||
{
|
||||
extended_vec[i] = vec[i];
|
||||
}
|
||||
int offset = vec.size();
|
||||
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
extended_vec[offset + i].setZero();
|
||||
}
|
||||
}
|
||||
|
||||
void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
|
||||
{
|
||||
extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
|
||||
for (int i = 0; i < residual.size(); ++i)
|
||||
{
|
||||
extended_residual[i] = residual[i];
|
||||
}
|
||||
int offset = residual.size();
|
||||
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
|
||||
extended_residual[offset + i].setZero();
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int n = 0; n < lm.m_num_nodes; ++n)
|
||||
{
|
||||
extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void initialize() {}
|
||||
|
||||
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
|
||||
void computeResidual(btScalar dt, TVStack& residual);
|
||||
|
||||
// add explicit force to the velocity
|
||||
void applyExplicitForce(TVStack& force);
|
||||
|
||||
// apply force to velocity and optionally reset the force to zero
|
||||
void applyForce(TVStack& force, bool setZero);
|
||||
|
||||
// compute the norm of the residual
|
||||
btScalar computeNorm(const TVStack& residual) const;
|
||||
|
||||
// compute one step of the solve (there is only one solve if the system is linear)
|
||||
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
|
||||
|
||||
// perform A*x = b
|
||||
void multiply(const TVStack& x, TVStack& b) const;
|
||||
|
||||
// set initial guess for CG solve
|
||||
void initialGuess(TVStack& dv, const TVStack& residual);
|
||||
|
||||
// reset data structure and reset dt
|
||||
void reinitialize(bool nodeUpdated, btScalar dt);
|
||||
|
||||
void setDt(btScalar dt);
|
||||
|
||||
// add friction force to residual
|
||||
void applyDynamicFriction(TVStack& r);
|
||||
|
||||
// add dv to velocity
|
||||
void updateVelocity(const TVStack& dv);
|
||||
|
||||
//set constraints as projections
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// update the projections and project the residual
|
||||
void project(TVStack& r)
|
||||
{
|
||||
BT_PROFILE("project");
|
||||
m_projection.project(r);
|
||||
}
|
||||
|
||||
// perform precondition M^(-1) x = b
|
||||
void precondition(const TVStack& x, TVStack& b)
|
||||
{
|
||||
m_preconditioner->operator()(x, b);
|
||||
}
|
||||
|
||||
// reindex all the vertices
|
||||
virtual void updateId()
|
||||
{
|
||||
size_t node_id = 0;
|
||||
size_t face_id = 0;
|
||||
m_nodes.clear();
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].index = node_id;
|
||||
m_nodes.push_back(&psb->m_nodes[j]);
|
||||
++node_id;
|
||||
}
|
||||
for (int j = 0; j < psb->m_faces.size(); ++j)
|
||||
{
|
||||
psb->m_faces[j].m_index = face_id;
|
||||
++face_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
|
||||
{
|
||||
return &m_nodes;
|
||||
}
|
||||
|
||||
void setImplicit(bool implicit)
|
||||
{
|
||||
m_implicit = implicit;
|
||||
}
|
||||
|
||||
// Calculate the total potential energy in the system
|
||||
btScalar totalEnergy(btScalar dt);
|
||||
|
||||
void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
|
||||
{
|
||||
extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
|
||||
for (int i = 0; i < vec.size(); ++i)
|
||||
{
|
||||
extended_vec[i] = vec[i];
|
||||
}
|
||||
int offset = vec.size();
|
||||
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
extended_vec[offset + i].setZero();
|
||||
}
|
||||
}
|
||||
|
||||
void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
|
||||
{
|
||||
extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
|
||||
for (int i = 0; i < residual.size(); ++i)
|
||||
{
|
||||
extended_residual[i] = residual[i];
|
||||
}
|
||||
int offset = residual.size();
|
||||
for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
|
||||
extended_residual[offset + i].setZero();
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int n = 0; n < lm.m_num_nodes; ++n)
|
||||
{
|
||||
extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
|
||||
{
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// add damping matrix
|
||||
m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
|
||||
}
|
||||
counter = 0;
|
||||
for (; counter < f.size(); ++counter)
|
||||
{
|
||||
f[counter] = rhs[counter] - f[counter];
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* btBackwardEulerObjective_h */
|
||||
|
@ -18,468 +18,489 @@
|
||||
#include "btDeformableBodySolver.h"
|
||||
#include "btSoftBodyInternals.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
static const int kMaxConjugateGradientIterations = 50;
|
||||
static const int kMaxConjugateGradientIterations = 300;
|
||||
btDeformableBodySolver::btDeformableBodySolver()
|
||||
: m_numNodes(0)
|
||||
, m_cg(kMaxConjugateGradientIterations)
|
||||
, m_cr(kMaxConjugateGradientIterations)
|
||||
, m_maxNewtonIterations(5)
|
||||
, m_newtonTolerance(1e-4)
|
||||
, m_lineSearch(false)
|
||||
, m_useProjection(false)
|
||||
: 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_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
|
||||
}
|
||||
|
||||
btDeformableBodySolver::~btDeformableBodySolver()
|
||||
{
|
||||
delete m_objective;
|
||||
delete m_objective;
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
|
||||
{
|
||||
BT_PROFILE("solveDeformableConstraints");
|
||||
if (!m_implicit)
|
||||
{
|
||||
m_objective->computeResidual(solverdt, m_residual);
|
||||
m_objective->applyDynamicFriction(m_residual);
|
||||
if (m_useProjection)
|
||||
{
|
||||
computeStep(m_dv, m_residual);
|
||||
}
|
||||
else
|
||||
{
|
||||
TVStack rhs, x;
|
||||
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
|
||||
m_objective->addLagrangeMultiplier(m_dv, x);
|
||||
m_objective->m_preconditioner->reinitialize(true);
|
||||
computeStep(x, rhs);
|
||||
for (int i = 0; i<m_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = x[i];
|
||||
}
|
||||
}
|
||||
updateVelocity();
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < m_maxNewtonIterations; ++i)
|
||||
{
|
||||
updateState();
|
||||
// add the inertia term in the residual
|
||||
int counter = 0;
|
||||
for (int k = 0; k < m_softBodies.size(); ++k)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[k];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (psb->m_nodes[j].m_im > 0)
|
||||
{
|
||||
m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter];
|
||||
}
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
BT_PROFILE("solveDeformableConstraints");
|
||||
if (!m_implicit)
|
||||
{
|
||||
m_objective->computeResidual(solverdt, m_residual);
|
||||
m_objective->applyDynamicFriction(m_residual);
|
||||
if (m_useProjection)
|
||||
{
|
||||
computeStep(m_dv, m_residual);
|
||||
}
|
||||
else
|
||||
{
|
||||
TVStack rhs, x;
|
||||
m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
|
||||
m_objective->addLagrangeMultiplier(m_dv, x);
|
||||
m_objective->m_preconditioner->reinitialize(true);
|
||||
computeStep(x, rhs);
|
||||
for (int i = 0; i < m_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = x[i];
|
||||
}
|
||||
}
|
||||
updateVelocity();
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < m_maxNewtonIterations; ++i)
|
||||
{
|
||||
updateState();
|
||||
// add the inertia term in the residual
|
||||
int counter = 0;
|
||||
for (int k = 0; k < m_softBodies.size(); ++k)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[k];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (psb->m_nodes[j].m_im > 0)
|
||||
{
|
||||
m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
|
||||
}
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
|
||||
m_objective->computeResidual(solverdt, m_residual);
|
||||
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
// todo xuchenhan@: this really only needs to be calculated once
|
||||
m_objective->applyDynamicFriction(m_residual);
|
||||
if (m_lineSearch)
|
||||
{
|
||||
btScalar inner_product = computeDescentStep(m_ddv,m_residual);
|
||||
btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
|
||||
btScalar scale = 2;
|
||||
btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
|
||||
backupDv();
|
||||
do {
|
||||
scale *= beta;
|
||||
if (scale < 1e-8) {
|
||||
return;
|
||||
}
|
||||
updateEnergy(scale);
|
||||
f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
|
||||
f2 = f0 - alpha * scale * inner_product;
|
||||
} while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
|
||||
revertDv();
|
||||
updateDv(scale);
|
||||
}
|
||||
else
|
||||
{
|
||||
computeStep(m_ddv, m_residual);
|
||||
updateDv();
|
||||
}
|
||||
for (int j = 0; j < m_numNodes; ++j)
|
||||
{
|
||||
m_ddv[j].setZero();
|
||||
m_residual[j].setZero();
|
||||
}
|
||||
}
|
||||
updateVelocity();
|
||||
}
|
||||
m_objective->computeResidual(solverdt, m_residual);
|
||||
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
// todo xuchenhan@: this really only needs to be calculated once
|
||||
m_objective->applyDynamicFriction(m_residual);
|
||||
if (m_lineSearch)
|
||||
{
|
||||
btScalar inner_product = computeDescentStep(m_ddv, m_residual);
|
||||
btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
|
||||
btScalar scale = 2;
|
||||
btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
|
||||
backupDv();
|
||||
do
|
||||
{
|
||||
scale *= beta;
|
||||
if (scale < 1e-8)
|
||||
{
|
||||
return;
|
||||
}
|
||||
updateEnergy(scale);
|
||||
f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
|
||||
f2 = f0 - alpha * scale * inner_product;
|
||||
} while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues
|
||||
revertDv();
|
||||
updateDv(scale);
|
||||
}
|
||||
else
|
||||
{
|
||||
computeStep(m_ddv, m_residual);
|
||||
updateDv();
|
||||
}
|
||||
for (int j = 0; j < m_numNodes; ++j)
|
||||
{
|
||||
m_ddv[j].setZero();
|
||||
m_residual[j].setZero();
|
||||
}
|
||||
}
|
||||
updateVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btDeformableBodySolver::kineticEnergy()
|
||||
{
|
||||
btScalar ke = 0;
|
||||
for (int i = 0; i < m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size();++j)
|
||||
{
|
||||
btSoftBody::Node& node = psb->m_nodes[j];
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
ke += m_dv[node.index].length2() * 0.5 / node.m_im;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ke;
|
||||
btScalar ke = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& node = psb->m_nodes[j];
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
ke += m_dv[node.index].length2() * 0.5 / node.m_im;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ke;
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::backupDv()
|
||||
{
|
||||
m_backup_dv.resize(m_dv.size());
|
||||
for (int i = 0; i<m_backup_dv.size(); ++i)
|
||||
{
|
||||
m_backup_dv[i] = m_dv[i];
|
||||
}
|
||||
m_backup_dv.resize(m_dv.size());
|
||||
for (int i = 0; i < m_backup_dv.size(); ++i)
|
||||
{
|
||||
m_backup_dv[i] = m_dv[i];
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::revertDv()
|
||||
{
|
||||
for (int i = 0; i<m_backup_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = m_backup_dv[i];
|
||||
}
|
||||
for (int i = 0; i < m_backup_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = m_backup_dv[i];
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::updateEnergy(btScalar scale)
|
||||
{
|
||||
for (int i = 0; i<m_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
|
||||
}
|
||||
updateState();
|
||||
for (int i = 0; i < m_dv.size(); ++i)
|
||||
{
|
||||
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
|
||||
}
|
||||
updateState();
|
||||
}
|
||||
|
||||
|
||||
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
|
||||
{
|
||||
m_cg.solve(*m_objective, ddv, residual, false);
|
||||
btScalar inner_product = m_cg.dot(residual, m_ddv);
|
||||
btScalar res_norm = m_objective->computeNorm(residual);
|
||||
btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
|
||||
if (inner_product < -tol)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Looking backwards!" << std::endl;
|
||||
}
|
||||
for (int i = 0; i < m_ddv.size();++i)
|
||||
{
|
||||
m_ddv[i] = -m_ddv[i];
|
||||
}
|
||||
inner_product = -inner_product;
|
||||
}
|
||||
else if (std::abs(inner_product) < tol)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Gradient Descent!" << std::endl;
|
||||
}
|
||||
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
|
||||
for (int i = 0; i < m_ddv.size();++i)
|
||||
{
|
||||
m_ddv[i] = scale * residual[i];
|
||||
}
|
||||
inner_product = scale * res_norm * res_norm;
|
||||
}
|
||||
return inner_product;
|
||||
m_cg.solve(*m_objective, ddv, residual, false);
|
||||
btScalar inner_product = m_cg.dot(residual, m_ddv);
|
||||
btScalar res_norm = m_objective->computeNorm(residual);
|
||||
btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
|
||||
if (inner_product < -tol)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Looking backwards!" << std::endl;
|
||||
}
|
||||
for (int i = 0; i < m_ddv.size(); ++i)
|
||||
{
|
||||
m_ddv[i] = -m_ddv[i];
|
||||
}
|
||||
inner_product = -inner_product;
|
||||
}
|
||||
else if (std::abs(inner_product) < tol)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
std::cout << "Gradient Descent!" << std::endl;
|
||||
}
|
||||
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
|
||||
for (int i = 0; i < m_ddv.size(); ++i)
|
||||
{
|
||||
m_ddv[i] = scale * residual[i];
|
||||
}
|
||||
inner_product = scale * res_norm * res_norm;
|
||||
}
|
||||
return inner_product;
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::updateState()
|
||||
{
|
||||
updateVelocity();
|
||||
updateTempPosition();
|
||||
updateVelocity();
|
||||
updateTempPosition();
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::updateDv(btScalar scale)
|
||||
{
|
||||
for (int i = 0; i < m_numNodes; ++i)
|
||||
{
|
||||
m_dv[i] += scale * m_ddv[i];
|
||||
}
|
||||
for (int i = 0; i < m_numNodes; ++i)
|
||||
{
|
||||
m_dv[i] += scale * m_ddv[i];
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
|
||||
{
|
||||
if (m_useProjection)
|
||||
m_cg.solve(*m_objective, ddv, residual, false);
|
||||
else
|
||||
m_cr.solve(*m_objective, ddv, residual, false);
|
||||
if (m_useProjection)
|
||||
m_cg.solve(*m_objective, ddv, residual, false);
|
||||
else
|
||||
m_cr.solve(*m_objective, ddv, residual, false);
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
|
||||
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
|
||||
{
|
||||
m_softBodies.copyFromArray(softBodies);
|
||||
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();
|
||||
}
|
||||
|
||||
m_dt = dt;
|
||||
m_objective->reinitialize(nodeUpdated, dt);
|
||||
updateSoftBodies();
|
||||
m_softBodies.copyFromArray(softBodies);
|
||||
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);
|
||||
updateSoftBodies();
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("setConstraint");
|
||||
m_objective->setConstraints(infoGlobal);
|
||||
BT_PROFILE("setConstraint");
|
||||
m_objective->setConstraints(infoGlobal);
|
||||
}
|
||||
|
||||
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("solveContactConstraints");
|
||||
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
|
||||
return maxSquaredResidual;
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
m_objective->m_projection.splitImpulseSetup(infoGlobal);
|
||||
BT_PROFILE("solveContactConstraints");
|
||||
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
|
||||
return maxSquaredResidual;
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::updateVelocity()
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
psb->m_maxSpeedSquared = 0;
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
// set NaN to zero;
|
||||
if (m_dv[counter] != m_dv[counter])
|
||||
{
|
||||
m_dv[counter].setZero();
|
||||
}
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
|
||||
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
psb->m_maxSpeedSquared = 0;
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
// set NaN to zero;
|
||||
if (m_dv[counter] != m_dv[counter])
|
||||
{
|
||||
m_dv[counter].setZero();
|
||||
}
|
||||
if (m_implicit)
|
||||
{
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
|
||||
}
|
||||
else
|
||||
{
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
|
||||
}
|
||||
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::updateTempPosition()
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
|
||||
++counter;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
|
||||
++counter;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::backupVelocity()
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
|
||||
}
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::setupDeformableSolve(bool implicit)
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (implicit)
|
||||
{
|
||||
if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
|
||||
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
|
||||
else
|
||||
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
|
||||
m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
|
||||
}
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter];
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
counter += psb->m_nodes.size();
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
if (implicit)
|
||||
{
|
||||
// setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
|
||||
if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
|
||||
m_dv[counter].setZero();
|
||||
else
|
||||
m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
|
||||
m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
|
||||
}
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter];
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::revertVelocity()
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter++];
|
||||
}
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_v = m_backupVelocity[counter++];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool btDeformableBodySolver::updateNodes()
|
||||
{
|
||||
int numNodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
numNodes += m_softBodies[i]->m_nodes.size();
|
||||
if (numNodes != m_numNodes)
|
||||
{
|
||||
m_numNodes = numNodes;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
int numNodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
numNodes += m_softBodies[i]->m_nodes.size();
|
||||
if (numNodes != m_numNodes)
|
||||
{
|
||||
m_numNodes = numNodes;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void btDeformableBodySolver::predictMotion(btScalar solverdt)
|
||||
{
|
||||
// apply explicit forces to velocity
|
||||
m_objective->applyExplicitForce(m_residual);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody *psb = m_softBodies[i];
|
||||
|
||||
if (psb->isActive())
|
||||
{
|
||||
// predict motion for collision detection
|
||||
predictDeformableMotion(psb, solverdt);
|
||||
}
|
||||
}
|
||||
// apply explicit forces to velocity
|
||||
if (m_implicit)
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
m_objective->applyExplicitForce(m_residual);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
|
||||
if (psb->isActive())
|
||||
{
|
||||
// predict motion for collision detection
|
||||
predictDeformableMotion(psb, solverdt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
|
||||
{
|
||||
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
|
||||
int i, ni;
|
||||
|
||||
/* Update */
|
||||
if (psb->m_bUpdateRtCst)
|
||||
{
|
||||
psb->m_bUpdateRtCst = false;
|
||||
psb->updateConstants();
|
||||
psb->m_fdbvt.clear();
|
||||
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
|
||||
{
|
||||
psb->initializeFaceTree();
|
||||
}
|
||||
}
|
||||
|
||||
/* Prepare */
|
||||
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
|
||||
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
|
||||
psb->m_sst.velmrg = psb->m_sst.sdt * 3;
|
||||
psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
|
||||
psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
|
||||
/* Bounds */
|
||||
psb->updateBounds();
|
||||
|
||||
/* Integrate */
|
||||
// do not allow particles to move more than the bounding box size
|
||||
btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt;
|
||||
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
|
||||
{
|
||||
btSoftBody::Node& n = psb->m_nodes[i];
|
||||
// apply drag
|
||||
n.m_v *= (1 - psb->m_cfg.drag);
|
||||
// scale velocity back
|
||||
if (n.m_v.norm() > max_v)
|
||||
{
|
||||
n.m_v.safeNormalize();
|
||||
n.m_v *= max_v;
|
||||
}
|
||||
n.m_q = n.m_x + n.m_v * dt;
|
||||
n.m_penetration = 0;
|
||||
}
|
||||
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
|
||||
int i, ni;
|
||||
|
||||
/* Nodes */
|
||||
psb->updateNodeTree(true, true);
|
||||
if (!psb->m_fdbvt.empty())
|
||||
{
|
||||
psb->updateFaceTree(true, true);
|
||||
}
|
||||
/* Clear contacts */
|
||||
psb->m_nodeRigidContacts.resize(0);
|
||||
psb->m_faceRigidContacts.resize(0);
|
||||
psb->m_faceNodeContacts.resize(0);
|
||||
/* Optimize dbvt's */
|
||||
// psb->m_ndbvt.optimizeIncremental(1);
|
||||
// psb->m_fdbvt.optimizeIncremental(1);
|
||||
/* Update */
|
||||
if (psb->m_bUpdateRtCst)
|
||||
{
|
||||
psb->m_bUpdateRtCst = false;
|
||||
psb->updateConstants();
|
||||
psb->m_fdbvt.clear();
|
||||
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
|
||||
{
|
||||
psb->initializeFaceTree();
|
||||
}
|
||||
}
|
||||
|
||||
/* Prepare */
|
||||
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
|
||||
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
|
||||
psb->m_sst.velmrg = psb->m_sst.sdt * 3;
|
||||
psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
|
||||
psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
|
||||
/* Bounds */
|
||||
psb->updateBounds();
|
||||
|
||||
/* Integrate */
|
||||
// do not allow particles to move more than the bounding box size
|
||||
btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
|
||||
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
|
||||
{
|
||||
btSoftBody::Node& n = psb->m_nodes[i];
|
||||
// apply drag
|
||||
n.m_v *= (1 - psb->m_cfg.drag);
|
||||
// scale velocity back
|
||||
if (m_implicit)
|
||||
{
|
||||
n.m_q = n.m_x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (n.m_v.norm() > max_v)
|
||||
{
|
||||
n.m_v.safeNormalize();
|
||||
n.m_v *= max_v;
|
||||
}
|
||||
n.m_q = n.m_x + n.m_v * dt;
|
||||
}
|
||||
n.m_splitv.setZero();
|
||||
n.m_constrained = false;
|
||||
}
|
||||
|
||||
/* Nodes */
|
||||
psb->updateNodeTree(true, true);
|
||||
if (!psb->m_fdbvt.empty())
|
||||
{
|
||||
psb->updateFaceTree(true, true);
|
||||
}
|
||||
/* Clear contacts */
|
||||
psb->m_nodeRigidContacts.resize(0);
|
||||
psb->m_faceRigidContacts.resize(0);
|
||||
psb->m_faceNodeContacts.resize(0);
|
||||
/* Optimize dbvt's */
|
||||
// psb->m_ndbvt.optimizeIncremental(1);
|
||||
// psb->m_fdbvt.optimizeIncremental(1);
|
||||
}
|
||||
|
||||
|
||||
void btDeformableBodySolver::updateSoftBodies()
|
||||
{
|
||||
BT_PROFILE("updateSoftBodies");
|
||||
for (int i = 0; i < m_softBodies.size(); i++)
|
||||
{
|
||||
btSoftBody *psb = (btSoftBody *)m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
psb->updateNormals();
|
||||
}
|
||||
}
|
||||
BT_PROFILE("updateSoftBodies");
|
||||
for (int i = 0; i < m_softBodies.size(); i++)
|
||||
{
|
||||
btSoftBody* psb = (btSoftBody*)m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
psb->updateNormals();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::setImplicit(bool implicit)
|
||||
{
|
||||
m_implicit = implicit;
|
||||
m_objective->setImplicit(implicit);
|
||||
m_implicit = implicit;
|
||||
m_objective->setImplicit(implicit);
|
||||
}
|
||||
|
||||
void btDeformableBodySolver::setLineSearch(bool lineSearch)
|
||||
{
|
||||
m_lineSearch = lineSearch;
|
||||
m_lineSearch = lineSearch;
|
||||
}
|
||||
|
@ -16,7 +16,6 @@
|
||||
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
|
||||
#define BT_DEFORMABLE_BODY_SOLVERS_H
|
||||
|
||||
|
||||
#include "btSoftBodySolvers.h"
|
||||
#include "btDeformableBackwardEulerObjective.h"
|
||||
#include "btDeformableMultiBodyDynamicsWorld.h"
|
||||
@ -30,133 +29,132 @@ class btDeformableMultiBodyDynamicsWorld;
|
||||
|
||||
class btDeformableBodySolver : public btSoftBodySolver
|
||||
{
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
|
||||
protected:
|
||||
int m_numNodes; // total number of deformable body nodes
|
||||
TVStack m_dv; // v_{n+1} - v_n
|
||||
TVStack m_backup_dv; // backed up dv
|
||||
TVStack m_ddv; // incremental dv
|
||||
TVStack m_residual; // rhs of the linear solve
|
||||
btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies
|
||||
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
|
||||
btScalar m_dt; // dt
|
||||
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
|
||||
btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver
|
||||
bool m_implicit; // use implicit scheme if true, explicit scheme if false
|
||||
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
|
||||
int m_numNodes; // total number of deformable body nodes
|
||||
TVStack m_dv; // v_{n+1} - v_n
|
||||
TVStack m_backup_dv; // backed up dv
|
||||
TVStack m_ddv; // incremental dv
|
||||
TVStack m_residual; // rhs of the linear solve
|
||||
btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies
|
||||
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
|
||||
btScalar m_dt; // dt
|
||||
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
|
||||
btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver
|
||||
bool m_implicit; // use implicit scheme if true, explicit scheme if false
|
||||
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
|
||||
public:
|
||||
// handles data related to objective function
|
||||
btDeformableBackwardEulerObjective* m_objective;
|
||||
bool m_useProjection;
|
||||
|
||||
btDeformableBodySolver();
|
||||
|
||||
virtual ~btDeformableBodySolver();
|
||||
|
||||
virtual SolverTypes getSolverType() const
|
||||
{
|
||||
return DEFORMABLE_SOLVER;
|
||||
}
|
||||
// handles data related to objective function
|
||||
btDeformableBackwardEulerObjective* m_objective;
|
||||
bool m_useProjection;
|
||||
|
||||
// update soft body normals
|
||||
virtual void updateSoftBodies();
|
||||
|
||||
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// solve the momentum equation
|
||||
virtual void solveDeformableConstraints(btScalar solverdt);
|
||||
|
||||
// set up the position error in split impulse
|
||||
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
|
||||
btDeformableBodySolver();
|
||||
|
||||
// resize/clear data structures
|
||||
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
|
||||
|
||||
// set up contact constraints
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
|
||||
virtual void predictMotion(btScalar solverdt);
|
||||
|
||||
// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
|
||||
// x_{n+1}^* is stored in m_q
|
||||
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
|
||||
|
||||
// save the current velocity to m_backupVelocity
|
||||
void backupVelocity();
|
||||
|
||||
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
||||
void setupDeformableSolve(bool implicit);
|
||||
|
||||
// set the current velocity to that backed up in m_backupVelocity
|
||||
void revertVelocity();
|
||||
|
||||
// set velocity to m_dv + m_backupVelocity
|
||||
void updateVelocity();
|
||||
|
||||
// update the node count
|
||||
bool updateNodes();
|
||||
|
||||
// calculate the change in dv resulting from the momentum solve
|
||||
void computeStep(TVStack& ddv, const TVStack& residual);
|
||||
|
||||
// calculate the change in dv resulting from the momentum solve when line search is turned on
|
||||
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false);
|
||||
virtual ~btDeformableBodySolver();
|
||||
|
||||
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
|
||||
virtual SolverTypes getSolverType() const
|
||||
{
|
||||
return DEFORMABLE_SOLVER;
|
||||
}
|
||||
|
||||
// process collision between deformable and rigid
|
||||
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
|
||||
{
|
||||
softBody->defaultCollisionHandler(collisionObjectWrap);
|
||||
}
|
||||
|
||||
// process collision between deformable and deformable
|
||||
virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
|
||||
softBody->defaultCollisionHandler(otherSoftBody);
|
||||
}
|
||||
// update soft body normals
|
||||
virtual void updateSoftBodies();
|
||||
|
||||
// If true, implicit time stepping scheme is used.
|
||||
// Otherwise, explicit time stepping scheme is used
|
||||
void setImplicit(bool implicit);
|
||||
|
||||
// If true, newton's method with line search is used when implicit time stepping scheme is turned on
|
||||
void setLineSearch(bool lineSearch);
|
||||
|
||||
// set temporary position x^* = x_n + dt * v
|
||||
// update the deformation gradient at position x^*
|
||||
void updateState();
|
||||
|
||||
// set dv = dv + scale * ddv
|
||||
void updateDv(btScalar scale = 1);
|
||||
|
||||
// set temporary position x^* = x_n + dt * v^*
|
||||
void updateTempPosition();
|
||||
|
||||
// save the current dv to m_backup_dv;
|
||||
void backupDv();
|
||||
|
||||
// set dv to the backed-up value
|
||||
void revertDv();
|
||||
|
||||
// set dv = dv + scale * ddv
|
||||
// set v^* = v_n + dv
|
||||
// set temporary position x^* = x_n + dt * v^*
|
||||
// update the deformation gradient at position x^*
|
||||
void updateEnergy(btScalar scale);
|
||||
|
||||
// calculates the appropriately scaled kinetic energy in the system, which is
|
||||
// 1/2 * dv^T * M * dv
|
||||
// used in line search
|
||||
btScalar kineticEnergy();
|
||||
|
||||
// unused functions
|
||||
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
|
||||
virtual void solveConstraints(btScalar dt){}
|
||||
virtual bool checkInitialized(){return true;}
|
||||
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
||||
virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// solve the momentum equation
|
||||
virtual void solveDeformableConstraints(btScalar solverdt);
|
||||
|
||||
// resize/clear data structures
|
||||
void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
|
||||
|
||||
// set up contact constraints
|
||||
void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
|
||||
virtual void predictMotion(btScalar solverdt);
|
||||
|
||||
// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
|
||||
// x_{n+1}^* is stored in m_q
|
||||
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
|
||||
|
||||
// save the current velocity to m_backupVelocity
|
||||
void backupVelocity();
|
||||
|
||||
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
||||
void setupDeformableSolve(bool implicit);
|
||||
|
||||
// set the current velocity to that backed up in m_backupVelocity
|
||||
void revertVelocity();
|
||||
|
||||
// set velocity to m_dv + m_backupVelocity
|
||||
void updateVelocity();
|
||||
|
||||
// update the node count
|
||||
bool updateNodes();
|
||||
|
||||
// calculate the change in dv resulting from the momentum solve
|
||||
void computeStep(TVStack& ddv, const TVStack& residual);
|
||||
|
||||
// calculate the change in dv resulting from the momentum solve when line search is turned on
|
||||
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false);
|
||||
|
||||
virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {}
|
||||
|
||||
// process collision between deformable and rigid
|
||||
virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap)
|
||||
{
|
||||
softBody->defaultCollisionHandler(collisionObjectWrap);
|
||||
}
|
||||
|
||||
// process collision between deformable and deformable
|
||||
virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
|
||||
{
|
||||
softBody->defaultCollisionHandler(otherSoftBody);
|
||||
}
|
||||
|
||||
// If true, implicit time stepping scheme is used.
|
||||
// Otherwise, explicit time stepping scheme is used
|
||||
void setImplicit(bool implicit);
|
||||
|
||||
// If true, newton's method with line search is used when implicit time stepping scheme is turned on
|
||||
void setLineSearch(bool lineSearch);
|
||||
|
||||
// set temporary position x^* = x_n + dt * v
|
||||
// update the deformation gradient at position x^*
|
||||
void updateState();
|
||||
|
||||
// set dv = dv + scale * ddv
|
||||
void updateDv(btScalar scale = 1);
|
||||
|
||||
// set temporary position x^* = x_n + dt * v^*
|
||||
void updateTempPosition();
|
||||
|
||||
// save the current dv to m_backup_dv;
|
||||
void backupDv();
|
||||
|
||||
// set dv to the backed-up value
|
||||
void revertDv();
|
||||
|
||||
// set dv = dv + scale * ddv
|
||||
// set v^* = v_n + dv
|
||||
// set temporary position x^* = x_n + dt * v^*
|
||||
// update the deformation gradient at position x^*
|
||||
void updateEnergy(btScalar scale);
|
||||
|
||||
// calculates the appropriately scaled kinetic energy in the system, which is
|
||||
// 1/2 * dv^T * M * dv
|
||||
// used in line search
|
||||
btScalar kineticEnergy();
|
||||
|
||||
// unused functions
|
||||
virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
|
||||
virtual void solveConstraints(btScalar dt) {}
|
||||
virtual bool checkInitialized() { return true; }
|
||||
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
||||
};
|
||||
|
||||
#endif /* btDeformableBodySolver_h */
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -21,51 +21,49 @@
|
||||
class btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
// True if the friction is static
|
||||
// False if the friction is dynamic
|
||||
bool m_static;
|
||||
// True if the friction is static
|
||||
// False if the friction is dynamic
|
||||
bool m_static;
|
||||
const btContactSolverInfo* m_infoGlobal;
|
||||
|
||||
// normal of the contact
|
||||
btVector3 m_normal;
|
||||
|
||||
btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableContactConstraint(){}
|
||||
|
||||
btDeformableContactConstraint() {}
|
||||
|
||||
btDeformableContactConstraint(const btDeformableContactConstraint& other)
|
||||
: m_static(other.m_static)
|
||||
, m_normal(other.m_normal)
|
||||
, m_infoGlobal(other.m_infoGlobal)
|
||||
: m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableContactConstraint(){}
|
||||
|
||||
// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
|
||||
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
|
||||
|
||||
// get the velocity of the object A in the contact
|
||||
virtual btVector3 getVa() const = 0;
|
||||
|
||||
// get the velocity of the object B in the contact
|
||||
virtual btVector3 getVb() const = 0;
|
||||
|
||||
// get the velocity change of the soft body node in the constraint
|
||||
virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
|
||||
|
||||
// apply impulse to the soft body node and/or face involved
|
||||
virtual void applyImpulse(const btVector3& impulse) = 0;
|
||||
|
||||
// scale the penetration depth by erp
|
||||
virtual void setPenetrationScale(btScalar scale) = 0;
|
||||
virtual ~btDeformableContactConstraint() {}
|
||||
|
||||
// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
|
||||
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
|
||||
|
||||
// get the velocity of the object A in the contact
|
||||
virtual btVector3 getVa() const = 0;
|
||||
|
||||
// get the velocity of the object B in the contact
|
||||
virtual btVector3 getVb() const = 0;
|
||||
|
||||
// get the velocity change of the soft body node in the constraint
|
||||
virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
|
||||
|
||||
// apply impulse to the soft body node and/or face involved
|
||||
virtual void applyImpulse(const btVector3& impulse) = 0;
|
||||
|
||||
// scale the penetration depth by erp
|
||||
virtual void setPenetrationScale(btScalar scale) = 0;
|
||||
};
|
||||
|
||||
//
|
||||
@ -73,42 +71,41 @@ public:
|
||||
class btDeformableStaticConstraint : public btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
btSoftBody::Node* m_node;
|
||||
|
||||
btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
|
||||
{
|
||||
}
|
||||
btDeformableStaticConstraint(){}
|
||||
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
|
||||
: m_node(other.m_node)
|
||||
, btDeformableContactConstraint(other)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableStaticConstraint(){}
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
btSoftBody::Node* m_node;
|
||||
|
||||
virtual btVector3 getVa() const
|
||||
{
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
|
||||
virtual btVector3 getVb() const
|
||||
{
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
|
||||
virtual btVector3 getDv(const btSoftBody::Node* n) const
|
||||
{
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse){}
|
||||
virtual void setPenetrationScale(btScalar scale){}
|
||||
btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal)
|
||||
{
|
||||
}
|
||||
btDeformableStaticConstraint() {}
|
||||
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
|
||||
: m_node(other.m_node), btDeformableContactConstraint(other)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableStaticConstraint() {}
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual btVector3 getVa() const
|
||||
{
|
||||
return btVector3(0, 0, 0);
|
||||
}
|
||||
|
||||
virtual btVector3 getVb() const
|
||||
{
|
||||
return btVector3(0, 0, 0);
|
||||
}
|
||||
|
||||
virtual btVector3 getDv(const btSoftBody::Node* n) const
|
||||
{
|
||||
return btVector3(0, 0, 0);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse) {}
|
||||
virtual void setPenetrationScale(btScalar scale) {}
|
||||
};
|
||||
|
||||
//
|
||||
@ -116,56 +113,67 @@ public:
|
||||
class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
|
||||
|
||||
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
|
||||
btDeformableNodeAnchorConstraint(){}
|
||||
virtual ~btDeformableNodeAnchorConstraint()
|
||||
{
|
||||
}
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
|
||||
|
||||
// object A is the rigid/multi body, and object B is the deformable node/face
|
||||
virtual btVector3 getVa() const;
|
||||
// get the velocity of the deformable node in contact
|
||||
virtual btVector3 getVb() const;
|
||||
virtual btVector3 getDv(const btSoftBody::Node* n) const
|
||||
{
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
|
||||
btDeformableNodeAnchorConstraint() {}
|
||||
virtual ~btDeformableNodeAnchorConstraint()
|
||||
{
|
||||
}
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale){}
|
||||
// object A is the rigid/multi body, and object B is the deformable node/face
|
||||
virtual btVector3 getVa() const;
|
||||
// get the velocity of the deformable node in contact
|
||||
virtual btVector3 getVb() const;
|
||||
virtual btVector3 getDv(const btSoftBody::Node* n) const
|
||||
{
|
||||
return btVector3(0, 0, 0);
|
||||
}
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale) {}
|
||||
};
|
||||
|
||||
|
||||
//
|
||||
// Constraint between rigid/multi body and deformable objects
|
||||
class btDeformableRigidContactConstraint : public btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
btVector3 m_total_normal_dv;
|
||||
btVector3 m_total_tangent_dv;
|
||||
btScalar m_penetration;
|
||||
const btSoftBody::DeformableRigidContact* m_contact;
|
||||
|
||||
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
|
||||
btDeformableRigidContactConstraint(){}
|
||||
virtual ~btDeformableRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// object A is the rigid/multi body, and object B is the deformable node/face
|
||||
virtual btVector3 getVa() const;
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale)
|
||||
{
|
||||
m_penetration *= scale;
|
||||
}
|
||||
btVector3 m_total_normal_dv;
|
||||
btVector3 m_total_tangent_dv;
|
||||
btScalar m_penetration;
|
||||
btScalar m_total_split_impulse;
|
||||
bool m_binding;
|
||||
const btSoftBody::DeformableRigidContact* m_contact;
|
||||
|
||||
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
|
||||
btDeformableRigidContactConstraint() {}
|
||||
virtual ~btDeformableRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// object A is the rigid/multi body, and object B is the deformable node/face
|
||||
virtual btVector3 getVa() const;
|
||||
|
||||
// get the split impulse velocity of the deformable face at the contact point
|
||||
virtual btVector3 getSplitVb() const = 0;
|
||||
|
||||
// get the split impulse velocity of the rigid/multibdoy at the contaft
|
||||
virtual btVector3 getSplitVa() const;
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale)
|
||||
{
|
||||
m_penetration *= scale;
|
||||
}
|
||||
|
||||
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void applySplitImpulse(const btVector3& impulse) = 0;
|
||||
};
|
||||
|
||||
//
|
||||
@ -173,29 +181,34 @@ public:
|
||||
class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
|
||||
{
|
||||
public:
|
||||
// the deformable node in contact
|
||||
btSoftBody::Node* m_node;
|
||||
|
||||
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
|
||||
btDeformableNodeRigidContactConstraint(){}
|
||||
virtual ~btDeformableNodeRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// get the velocity of the deformable node in contact
|
||||
virtual btVector3 getVb() 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);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
// the deformable node in contact
|
||||
btSoftBody::Node* m_node;
|
||||
|
||||
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
|
||||
btDeformableNodeRigidContactConstraint() {}
|
||||
virtual ~btDeformableNodeRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// get the velocity of the deformable node in contact
|
||||
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::DeformableNodeRigidContact* getContact() const
|
||||
{
|
||||
return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
|
||||
virtual void applySplitImpulse(const btVector3& impulse);
|
||||
};
|
||||
|
||||
//
|
||||
@ -203,28 +216,33 @@ public:
|
||||
class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
|
||||
{
|
||||
public:
|
||||
const btSoftBody::Face* m_face;
|
||||
bool m_useStrainLimiting;
|
||||
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
|
||||
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
|
||||
btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {}
|
||||
virtual ~btDeformableFaceRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// get the velocity of the deformable face at the contact point
|
||||
virtual btVector3 getVb() 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);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
btSoftBody::Face* m_face;
|
||||
bool m_useStrainLimiting;
|
||||
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
|
||||
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
|
||||
btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {}
|
||||
virtual ~btDeformableFaceRigidContactConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
|
||||
virtual void applySplitImpulse(const btVector3& impulse);
|
||||
};
|
||||
|
||||
//
|
||||
@ -232,35 +250,35 @@ public:
|
||||
class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
|
||||
{
|
||||
public:
|
||||
btSoftBody::Node* m_node;
|
||||
btSoftBody::Face* m_face;
|
||||
const btSoftBody::DeformableFaceNodeContact* m_contact;
|
||||
btVector3 m_total_normal_dv;
|
||||
btVector3 m_total_tangent_dv;
|
||||
|
||||
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableFaceNodeContactConstraint(){}
|
||||
virtual ~btDeformableFaceNodeContactConstraint(){}
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// get the velocity of the object A in the contact
|
||||
virtual btVector3 getVa() const;
|
||||
|
||||
// get the velocity of the object B in the contact
|
||||
virtual btVector3 getVb() 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::DeformableFaceNodeContact* getContact() const
|
||||
{
|
||||
return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
btSoftBody::Node* m_node;
|
||||
btSoftBody::Face* m_face;
|
||||
const btSoftBody::DeformableFaceNodeContact* m_contact;
|
||||
btVector3 m_total_normal_dv;
|
||||
btVector3 m_total_tangent_dv;
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale){}
|
||||
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
|
||||
btDeformableFaceNodeContactConstraint() {}
|
||||
virtual ~btDeformableFaceNodeContactConstraint() {}
|
||||
|
||||
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// get the velocity of the object A in the contact
|
||||
virtual btVector3 getVa() const;
|
||||
|
||||
// get the velocity of the object B in the contact
|
||||
virtual btVector3 getVb() 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::DeformableFaceNodeContact* getContact() const
|
||||
{
|
||||
return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
|
||||
}
|
||||
|
||||
virtual void applyImpulse(const btVector3& impulse);
|
||||
|
||||
virtual void setPenetrationScale(btScalar scale) {}
|
||||
};
|
||||
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */
|
||||
|
@ -17,7 +17,7 @@
|
||||
#include "btDeformableMultiBodyDynamicsWorld.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
btScalar residualSquare = 0;
|
||||
for (int i = 0; i < numDeformableBodies; ++i)
|
||||
@ -58,27 +58,37 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
|
||||
return residualSquare;
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
|
||||
btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
btScalar residualSquare = 0;
|
||||
for (int i = 0; i < numDeformableBodies; ++i)
|
||||
{
|
||||
// node constraints
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
for (int j = 0; j < m_softBodies.size(); ++j)
|
||||
{
|
||||
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
|
||||
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
|
||||
}
|
||||
// face constraints
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
|
||||
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
|
||||
btCollisionObject* psb = m_softBodies[j];
|
||||
if (psb != deformableBodies[i])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
|
||||
{
|
||||
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
|
||||
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
|
||||
residualSquare = btMax(residualSquare, localResidualSquare);
|
||||
}
|
||||
}
|
||||
}
|
||||
return residualSquare;
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
{
|
||||
BT_PROFILE("setConstraints");
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
|
||||
m_staticConstraints[i].push_back(static_constraint);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// set up deformable anchors
|
||||
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
|
||||
{
|
||||
@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
|
||||
btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
|
||||
m_nodeAnchorConstraints[i].push_back(constraint);
|
||||
}
|
||||
|
||||
|
||||
// set Deformable Node vs. Rigid constraint
|
||||
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
|
||||
{
|
||||
@ -122,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
|
||||
continue;
|
||||
}
|
||||
btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
|
||||
btVector3 va = constraint.getVa();
|
||||
btVector3 vb = constraint.getVb();
|
||||
const btVector3 vr = vb - va;
|
||||
const btSoftBody::sCti& cti = contact.m_cti;
|
||||
const btScalar dn = btDot(vr, cti.m_normal);
|
||||
if (dn < SIMD_EPSILON)
|
||||
{
|
||||
m_nodeRigidConstraints[i].push_back(constraint);
|
||||
}
|
||||
m_nodeRigidConstraints[i].push_back(constraint);
|
||||
}
|
||||
|
||||
|
||||
// set Deformable Face vs. Rigid constraint
|
||||
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
|
||||
{
|
||||
@ -143,15 +145,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
|
||||
continue;
|
||||
}
|
||||
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
|
||||
btVector3 va = constraint.getVa();
|
||||
btVector3 vb = constraint.getVb();
|
||||
const btVector3 vr = vb - va;
|
||||
const btSoftBody::sCti& cti = contact.m_cti;
|
||||
const btScalar dn = btDot(vr, cti.m_normal);
|
||||
if (dn < SIMD_EPSILON)
|
||||
{
|
||||
m_faceRigidConstraints[i].push_back(constraint);
|
||||
}
|
||||
m_faceRigidConstraints[i].push_back(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
|
||||
void btDeformableContactProjection::project(TVStack& x)
|
||||
{
|
||||
#ifndef USE_MGS
|
||||
const int dim = 3;
|
||||
for (int index = 0; index < m_projectionsDict.size(); ++index)
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
|
||||
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
|
||||
if (projectionDirs.size() >= dim)
|
||||
{
|
||||
// static node
|
||||
x[i].setZero();
|
||||
continue;
|
||||
}
|
||||
else if (projectionDirs.size() == 2)
|
||||
{
|
||||
btVector3 dir0 = projectionDirs[0];
|
||||
btVector3 dir1 = projectionDirs[1];
|
||||
btVector3 free_dir = btCross(dir0, dir1);
|
||||
if (free_dir.safeNorm() < SIMD_EPSILON)
|
||||
{
|
||||
x[i] -= x[i].dot(dir0) * dir0;
|
||||
x[i] -= x[i].dot(dir1) * dir1;
|
||||
}
|
||||
else
|
||||
{
|
||||
free_dir.normalize();
|
||||
x[i] = x[i].dot(free_dir) * free_dir;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(projectionDirs.size() == 1);
|
||||
btVector3 dir0 = projectionDirs[0];
|
||||
x[i] -= x[i].dot(dir0) * dir0;
|
||||
}
|
||||
}
|
||||
const int dim = 3;
|
||||
for (int index = 0; index < m_projectionsDict.size(); ++index)
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
|
||||
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
|
||||
if (projectionDirs.size() >= dim)
|
||||
{
|
||||
// static node
|
||||
x[i].setZero();
|
||||
continue;
|
||||
}
|
||||
else if (projectionDirs.size() == 2)
|
||||
{
|
||||
btVector3 dir0 = projectionDirs[0];
|
||||
btVector3 dir1 = projectionDirs[1];
|
||||
btVector3 free_dir = btCross(dir0, dir1);
|
||||
if (free_dir.safeNorm() < SIMD_EPSILON)
|
||||
{
|
||||
x[i] -= x[i].dot(dir0) * dir0;
|
||||
}
|
||||
else
|
||||
{
|
||||
free_dir.normalize();
|
||||
x[i] = x[i].dot(free_dir) * free_dir;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(projectionDirs.size() == 1);
|
||||
btVector3 dir0 = projectionDirs[0];
|
||||
x[i] -= x[i].dot(dir0) * dir0;
|
||||
}
|
||||
}
|
||||
#else
|
||||
btReducedVector p(x.size());
|
||||
for (int i = 0; i < m_projections.size(); ++i)
|
||||
{
|
||||
p += (m_projections[i].dot(x) * m_projections[i]);
|
||||
}
|
||||
for (int i = 0; i < p.m_indices.size(); ++i)
|
||||
{
|
||||
x[p.m_indices[i]] -= p.m_vecs[i];
|
||||
}
|
||||
btReducedVector p(x.size());
|
||||
for (int i = 0; i < m_projections.size(); ++i)
|
||||
{
|
||||
p += (m_projections[i].dot(x) * m_projections[i]);
|
||||
}
|
||||
for (int i = 0; i < p.m_indices.size(); ++i)
|
||||
{
|
||||
x[p.m_indices[i]] -= p.m_vecs[i];
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::setProjection()
|
||||
{
|
||||
#ifndef USE_MGS
|
||||
BT_PROFILE("btDeformableContactProjection::setProjection");
|
||||
btAlignedObjectArray<btVector3> units;
|
||||
units.push_back(btVector3(1,0,0));
|
||||
units.push_back(btVector3(0,1,0));
|
||||
units.push_back(btVector3(0,0,1));
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
btAlignedObjectArray<btVector3> projections;
|
||||
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
m_projectionsDict.insert(index, projections);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
|
||||
}
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
btSoftBody::Node* node = face->m_n[k];
|
||||
node->m_penetration = true;
|
||||
int index = node->index;
|
||||
if (m_faceRigidConstraints[i][j].m_static)
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
btAlignedObjectArray<btVector3> projections;
|
||||
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
|
||||
m_projectionsDict.insert(index, projections);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
BT_PROFILE("btDeformableContactProjection::setProjection");
|
||||
btAlignedObjectArray<btVector3> units;
|
||||
units.push_back(btVector3(1, 0, 0));
|
||||
units.push_back(btVector3(0, 1, 0));
|
||||
units.push_back(btVector3(0, 0, 1));
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_constrained = true;
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
|
||||
if (m_nodeRigidConstraints[i][j].m_binding)
|
||||
{
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
projections.push_back(units[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
btAlignedObjectArray<btVector3> projections;
|
||||
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
m_projectionsDict.insert(index, projections);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
if (m_faceRigidConstraints[i][j].m_binding)
|
||||
{
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
face->m_n[k]->m_constrained = true;
|
||||
}
|
||||
}
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
btSoftBody::Node* node = face->m_n[k];
|
||||
int index = node->index;
|
||||
if (m_faceRigidConstraints[i][j].m_static)
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
m_projectionsDict.insert(index, units);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
for (int l = 0; l < 3; ++l)
|
||||
{
|
||||
projections.push_back(units[l]);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_projectionsDict.find(index) == NULL)
|
||||
{
|
||||
btAlignedObjectArray<btVector3> projections;
|
||||
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
|
||||
m_projectionsDict.insert(index, projections);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
|
||||
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
int dof = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
dof += m_softBodies[i]->m_nodes.size();
|
||||
}
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
|
||||
indices.push_back(index);
|
||||
vecs1.push_back(btVector3(1,0,0));
|
||||
vecs2.push_back(btVector3(0,1,0));
|
||||
vecs3.push_back(btVector3(0,0,1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
|
||||
indices.push_back(index);
|
||||
vecs1.push_back(btVector3(1,0,0));
|
||||
vecs2.push_back(btVector3(0,1,0));
|
||||
vecs3.push_back(btVector3(0,0,1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
btAlignedObjectArray<int> indices;
|
||||
indices.push_back(index);
|
||||
btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
vecs1.push_back(btVector3(1,0,0));
|
||||
vecs2.push_back(btVector3(0,1,0));
|
||||
vecs3.push_back(btVector3(0,0,1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
else
|
||||
{
|
||||
vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
int dof = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
dof += m_softBodies[i]->m_nodes.size();
|
||||
}
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
||||
indices.push_back(index);
|
||||
vecs1.push_back(btVector3(1, 0, 0));
|
||||
vecs2.push_back(btVector3(0, 1, 0));
|
||||
vecs3.push_back(btVector3(0, 0, 1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
||||
indices.push_back(index);
|
||||
vecs1.push_back(btVector3(1, 0, 0));
|
||||
vecs2.push_back(btVector3(0, 1, 0));
|
||||
vecs3.push_back(btVector3(0, 0, 1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
btAlignedObjectArray<int> indices;
|
||||
indices.push_back(index);
|
||||
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
vecs1.push_back(btVector3(1, 0, 0));
|
||||
vecs2.push_back(btVector3(0, 1, 0));
|
||||
vecs3.push_back(btVector3(0, 0, 1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
||||
}
|
||||
else
|
||||
{
|
||||
vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
||||
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
|
||||
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
|
||||
}
|
||||
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
|
||||
}
|
||||
if (m_faceRigidConstraints[i][j].m_static)
|
||||
{
|
||||
for (int l = 0; l < 3; ++l)
|
||||
{
|
||||
|
||||
btReducedVector rv(dof);
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
rv.m_indices.push_back(face->m_n[k]->index);
|
||||
btVector3 v(0,0,0);
|
||||
btVector3 v(0, 0, 0);
|
||||
v[l] = bary[k];
|
||||
rv.m_vecs.push_back(v);
|
||||
rv.sort();
|
||||
rv.sort();
|
||||
}
|
||||
m_projections.push_back(rv);
|
||||
}
|
||||
@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection()
|
||||
{
|
||||
rv.m_indices.push_back(face->m_n[k]->index);
|
||||
rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
|
||||
rv.sort();
|
||||
rv.sort();
|
||||
}
|
||||
m_projections.push_back(rv);
|
||||
}
|
||||
}
|
||||
}
|
||||
btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
|
||||
mgs.solve();
|
||||
m_projections = mgs.m_out;
|
||||
}
|
||||
btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
|
||||
mgs.solve();
|
||||
m_projections = mgs.m_out;
|
||||
#endif
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::checkConstraints(const TVStack& x)
|
||||
{
|
||||
for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
btVector3 d(0,0,0);
|
||||
const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
for (int k = 0; k < lm.m_num_nodes; ++k)
|
||||
{
|
||||
d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
|
||||
}
|
||||
}
|
||||
printf("d = %f, %f, %f\n",d[0],d[1],d[2]);
|
||||
}
|
||||
for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
|
||||
{
|
||||
btVector3 d(0, 0, 0);
|
||||
const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
for (int k = 0; k < lm.m_num_nodes; ++k)
|
||||
{
|
||||
d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
|
||||
}
|
||||
}
|
||||
// printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
|
||||
// printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableContactProjection::setLagrangeMultiplier()
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1,0,0);
|
||||
lm.m_dirs[1] = btVector3(0,1,0);
|
||||
lm.m_dirs[2] = btVector3(0,0,1);
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1,0,0);
|
||||
lm.m_dirs[1] = btVector3(0,1,0);
|
||||
lm.m_dirs[2] = btVector3(0,0,1);
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1,0,0);
|
||||
lm.m_dirs[1] = btVector3(0,1,0);
|
||||
lm.m_dirs[2] = btVector3(0,0,1);
|
||||
}
|
||||
else
|
||||
{
|
||||
lm.m_num_constraints = 1;
|
||||
lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
|
||||
}
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
|
||||
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
|
||||
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_staticConstraints[i][j].m_node->index;
|
||||
m_staticConstraints[i][j].m_node->m_constrained = true;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 3;
|
||||
for (int k = 0; k<3; ++k)
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1, 0, 0);
|
||||
lm.m_dirs[1] = btVector3(0, 1, 0);
|
||||
lm.m_dirs[2] = btVector3(0, 0, 1);
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
||||
{
|
||||
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
||||
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1, 0, 0);
|
||||
lm.m_dirs[1] = btVector3(0, 1, 0);
|
||||
lm.m_dirs[2] = btVector3(0, 0, 1);
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
|
||||
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
if (!m_nodeRigidConstraints[i][j].m_binding)
|
||||
{
|
||||
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
|
||||
lm.m_indices[k] = face->m_n[k]->index;
|
||||
lm.m_weights[k] = bary[k];
|
||||
continue;
|
||||
}
|
||||
if (m_faceRigidConstraints[i][j].m_static)
|
||||
{
|
||||
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
||||
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 1;
|
||||
lm.m_indices[0] = index;
|
||||
lm.m_weights[0] = 1.0;
|
||||
if (m_nodeRigidConstraints[i][j].m_static)
|
||||
{
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1,0,0);
|
||||
lm.m_dirs[1] = btVector3(0,1,0);
|
||||
lm.m_dirs[2] = btVector3(0,0,1);
|
||||
lm.m_dirs[0] = btVector3(1, 0, 0);
|
||||
lm.m_dirs[1] = btVector3(0, 1, 0);
|
||||
lm.m_dirs[2] = btVector3(0, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
lm.m_num_constraints = 1;
|
||||
lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
|
||||
}
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
{
|
||||
if (!m_faceRigidConstraints[i][j].m_binding)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
||||
|
||||
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
|
||||
LagrangeMultiplier lm;
|
||||
lm.m_num_nodes = 3;
|
||||
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
face->m_n[k]->m_constrained = true;
|
||||
lm.m_indices[k] = face->m_n[k]->index;
|
||||
lm.m_weights[k] = bary[k];
|
||||
}
|
||||
if (m_faceRigidConstraints[i][j].m_static)
|
||||
{
|
||||
face->m_pcontact[3] = 1;
|
||||
lm.m_num_constraints = 3;
|
||||
lm.m_dirs[0] = btVector3(1, 0, 0);
|
||||
lm.m_dirs[1] = btVector3(0, 1, 0);
|
||||
lm.m_dirs[2] = btVector3(0, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
face->m_pcontact[3] = 0;
|
||||
lm.m_num_constraints = 1;
|
||||
lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
|
||||
}
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
m_lagrangeMultipliers.push_back(lm);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
|
||||
if (node->m_im != 0)
|
||||
{
|
||||
int index = node->index;
|
||||
f[index] += constraint.getDv(node)* (1./node->m_im);
|
||||
f[index] += constraint.getDv(node) * (1. / node->m_im);
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
||||
@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
|
||||
if (node->m_im != 0)
|
||||
{
|
||||
int index = node->index;
|
||||
f[index] += constraint.getDv(node)* (1./node->m_im);
|
||||
f[index] += constraint.getDv(node) * (1. / node->m_im);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
|
||||
if (node->m_im != 0)
|
||||
{
|
||||
int index = node->index;
|
||||
f[index] += constraint.getDv(node)* (1./node->m_im);
|
||||
f[index] += constraint.getDv(node) * (1. / node->m_im);
|
||||
}
|
||||
for (int k = 0; k < 3; ++k)
|
||||
{
|
||||
@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
|
||||
if (node->m_im != 0)
|
||||
{
|
||||
int index = node->index;
|
||||
f[index] += constraint.getDv(node)* (1./node->m_im);
|
||||
f[index] += constraint.getDv(node) * (1. / node->m_im);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
|
||||
m_nodeRigidConstraints.resize(N);
|
||||
m_faceRigidConstraints.resize(N);
|
||||
m_deformableConstraints.resize(N);
|
||||
|
||||
}
|
||||
for (int i = 0 ; i < N; ++i)
|
||||
for (int i = 0; i < N; ++i)
|
||||
{
|
||||
m_staticConstraints[i].clear();
|
||||
m_nodeAnchorConstraints[i].clear();
|
||||
@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
|
||||
m_deformableConstraints[i].clear();
|
||||
}
|
||||
#ifndef USE_MGS
|
||||
m_projectionsDict.clear();
|
||||
m_projectionsDict.clear();
|
||||
#else
|
||||
m_projections.clear();
|
||||
m_projections.clear();
|
||||
#endif
|
||||
m_lagrangeMultipliers.clear();
|
||||
m_lagrangeMultipliers.clear();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
@ -27,31 +27,30 @@
|
||||
|
||||
struct LagrangeMultiplier
|
||||
{
|
||||
int m_num_constraints; // Number of constraints
|
||||
int m_num_nodes; // Number of nodes in these constraints
|
||||
btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes
|
||||
btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints;
|
||||
int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes;
|
||||
int m_num_constraints; // Number of constraints
|
||||
int m_num_nodes; // Number of nodes in these constraints
|
||||
btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes
|
||||
btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints;
|
||||
int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes;
|
||||
};
|
||||
|
||||
|
||||
class btDeformableContactProjection
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
|
||||
// all constraints involving face
|
||||
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btAlignedObjectArray<btSoftBody*>& m_softBodies;
|
||||
|
||||
// all constraints involving face
|
||||
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
|
||||
#ifndef USE_MGS
|
||||
// map from node index to projection directions
|
||||
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
|
||||
// map from node index to projection directions
|
||||
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
|
||||
#else
|
||||
btAlignedObjectArray<btReducedVector> m_projections;
|
||||
btAlignedObjectArray<btReducedVector> m_projections;
|
||||
#endif
|
||||
|
||||
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
|
||||
|
||||
|
||||
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
|
||||
|
||||
// map from node index to static constraint
|
||||
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
|
||||
// map from node index to node rigid constraint
|
||||
@ -62,39 +61,39 @@ public:
|
||||
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
|
||||
// map from node index to node anchor constraint
|
||||
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
|
||||
|
||||
bool m_useStrainLimiting;
|
||||
|
||||
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
|
||||
: m_softBodies(softBodies)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableContactProjection()
|
||||
{
|
||||
}
|
||||
|
||||
// apply the constraints to the rhs of the linear solve
|
||||
virtual void project(TVStack& x);
|
||||
|
||||
// add friction force to the rhs of the linear solve
|
||||
virtual void applyDynamicFriction(TVStack& f);
|
||||
|
||||
// update and solve the constraints
|
||||
virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
|
||||
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Set up projections for each vertex by adding the projection direction to
|
||||
virtual void setProjection();
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated);
|
||||
|
||||
virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setLagrangeMultiplier();
|
||||
|
||||
void checkConstraints(const TVStack& x);
|
||||
|
||||
bool m_useStrainLimiting;
|
||||
|
||||
btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
|
||||
: m_softBodies(softBodies)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableContactProjection()
|
||||
{
|
||||
}
|
||||
|
||||
// apply the constraints to the rhs of the linear solve
|
||||
virtual void project(TVStack& x);
|
||||
|
||||
// add friction force to the rhs of the linear solve
|
||||
virtual void applyDynamicFriction(TVStack& f);
|
||||
|
||||
// update and solve the constraints
|
||||
virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
|
||||
virtual void setConstraints(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// Set up projections for each vertex by adding the projection direction to
|
||||
virtual void setProjection();
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated);
|
||||
|
||||
btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setLagrangeMultiplier();
|
||||
|
||||
void checkConstraints(const TVStack& x);
|
||||
};
|
||||
#endif /* btDeformableContactProjection_h */
|
||||
|
@ -21,107 +21,104 @@
|
||||
|
||||
static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
|
||||
{
|
||||
static const btPolarDecomposition polar;
|
||||
return polar.decompose(m, q, s);
|
||||
static const btPolarDecomposition polar;
|
||||
return polar.decompose(m, q, s);
|
||||
}
|
||||
|
||||
class btDeformableCorotatedForce : public btDeformableLagrangianForce
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda;
|
||||
btDeformableCorotatedForce(): m_mu(1), m_lambda(1)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(tetra.m_F,P);
|
||||
btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
// explicit elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
|
||||
{
|
||||
// btMatrix3x3 JFinvT = F.adjoint();
|
||||
btScalar J = F.determinant();
|
||||
P = F.adjoint().transpose() * (m_lambda * (J-1));
|
||||
if (m_mu > SIMD_EPSILON)
|
||||
{
|
||||
btMatrix3x3 R,S;
|
||||
if (J < 1024 * SIMD_EPSILON)
|
||||
R.setIdentity();
|
||||
else
|
||||
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
|
||||
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
|
||||
P += (F-R) * 2 * m_mu;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_COROTATED_FORCE;
|
||||
}
|
||||
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda;
|
||||
btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
|
||||
{
|
||||
}
|
||||
|
||||
btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(tetra.m_F, P);
|
||||
btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
// explicit elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
|
||||
{
|
||||
// btMatrix3x3 JFinvT = F.adjoint();
|
||||
btScalar J = F.determinant();
|
||||
P = F.adjoint().transpose() * (m_lambda * (J - 1));
|
||||
if (m_mu > SIMD_EPSILON)
|
||||
{
|
||||
btMatrix3x3 R, S;
|
||||
if (J < 1024 * SIMD_EPSILON)
|
||||
R.setIdentity();
|
||||
else
|
||||
PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
|
||||
/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
|
||||
P += (F - R) * 2 * m_mu;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_COROTATED_FORCE;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif /* btCorotated_h */
|
||||
|
@ -21,87 +21,85 @@
|
||||
class btDeformableGravityForce : public btDeformableLagrangianForce
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btVector3 m_gravity;
|
||||
|
||||
btDeformableGravityForce(const btVector3& g) : m_gravity(g)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledGravityForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledGravityForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
|
||||
|
||||
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& n = psb->m_nodes[j];
|
||||
size_t id = n.index;
|
||||
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
|
||||
btVector3 scaled_force = scale * m_gravity * mass;
|
||||
force[id] += scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_GRAVITY_FORCE;
|
||||
}
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btVector3 m_gravity;
|
||||
|
||||
// the gravitational potential energy
|
||||
virtual double totalEnergy(btScalar dt)
|
||||
{
|
||||
double e = 0;
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
e -= m_gravity.dot(node.m_q)/node.m_im;
|
||||
}
|
||||
}
|
||||
}
|
||||
return e;
|
||||
}
|
||||
|
||||
|
||||
btDeformableGravityForce(const btVector3& g) : m_gravity(g)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledGravityForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledGravityForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
|
||||
|
||||
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& n = psb->m_nodes[j];
|
||||
size_t id = n.index;
|
||||
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
|
||||
btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor;
|
||||
force[id] += scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_GRAVITY_FORCE;
|
||||
}
|
||||
|
||||
// the gravitational potential energy
|
||||
virtual double totalEnergy(btScalar dt)
|
||||
{
|
||||
double e = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
e -= m_gravity.dot(node.m_q) / node.m_im;
|
||||
}
|
||||
}
|
||||
}
|
||||
return e;
|
||||
}
|
||||
};
|
||||
#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */
|
||||
|
@ -22,352 +22,351 @@
|
||||
|
||||
enum btDeformableLagrangianForceType
|
||||
{
|
||||
BT_GRAVITY_FORCE = 1,
|
||||
BT_MASSSPRING_FORCE = 2,
|
||||
BT_COROTATED_FORCE = 3,
|
||||
BT_NEOHOOKEAN_FORCE = 4,
|
||||
BT_LINEAR_ELASTICITY_FORCE = 5,
|
||||
BT_MOUSE_PICKING_FORCE = 6
|
||||
BT_GRAVITY_FORCE = 1,
|
||||
BT_MASSSPRING_FORCE = 2,
|
||||
BT_COROTATED_FORCE = 3,
|
||||
BT_NEOHOOKEAN_FORCE = 4,
|
||||
BT_LINEAR_ELASTICITY_FORCE = 5,
|
||||
BT_MOUSE_PICKING_FORCE = 6
|
||||
};
|
||||
|
||||
static inline double randomDouble(double low, double high)
|
||||
{
|
||||
return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
|
||||
return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
|
||||
}
|
||||
|
||||
class btDeformableLagrangianForce
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btAlignedObjectArray<btSoftBody *> m_softBodies;
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
|
||||
|
||||
btDeformableLagrangianForce()
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btDeformableLagrangianForce(){}
|
||||
|
||||
// add all forces
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
// add damping df
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
|
||||
|
||||
// build diagonal of A matrix
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
|
||||
|
||||
// add elastic df
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
|
||||
|
||||
// add all forces that are explicit in explicit solve
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
// add all damping forces
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType() = 0;
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
// get number of nodes that have the force
|
||||
virtual int getNumNodes()
|
||||
{
|
||||
int numNodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
numNodes += m_softBodies[i]->m_nodes.size();
|
||||
}
|
||||
return numNodes;
|
||||
}
|
||||
|
||||
// add a soft body to be affected by the particular lagrangian force
|
||||
virtual void addSoftBody(btSoftBody* psb)
|
||||
{
|
||||
m_softBodies.push_back(psb);
|
||||
}
|
||||
|
||||
virtual void removeSoftBody(btSoftBody* psb)
|
||||
{
|
||||
m_softBodies.remove(psb);
|
||||
}
|
||||
|
||||
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
|
||||
{
|
||||
m_nodes = nodes;
|
||||
}
|
||||
|
||||
// Calculate the incremental deformable generated from the input dx
|
||||
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
|
||||
{
|
||||
btVector3 c1 = dx[id1] - dx[id0];
|
||||
btVector3 c2 = dx[id2] - dx[id0];
|
||||
btVector3 c3 = dx[id3] - dx[id0];
|
||||
return btMatrix3x3(c1,c2,c3).transpose();
|
||||
}
|
||||
|
||||
// Calculate the incremental deformable generated from the current velocity
|
||||
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
|
||||
{
|
||||
btVector3 c1 = n1->m_v - n0->m_v;
|
||||
btVector3 c2 = n2->m_v - n0->m_v;
|
||||
btVector3 c3 = n3->m_v - n0->m_v;
|
||||
return btMatrix3x3(c1,c2,c3).transpose();
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testDerivative()
|
||||
{
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
|
||||
TVStack dx;
|
||||
dx.resize(getNumNodes());
|
||||
TVStack dphi_dx;
|
||||
dphi_dx.resize(dx.size());
|
||||
for (int i =0; i < dphi_dx.size();++i)
|
||||
{
|
||||
dphi_dx[i].setZero();
|
||||
}
|
||||
addScaledForces(-1, dphi_dx);
|
||||
|
||||
// write down the current position
|
||||
TVStack x;
|
||||
x.resize(dx.size());
|
||||
int counter = 0;
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
x[counter] = psb->m_nodes[j].m_q;
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
// populate dx with random vectors
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i].setX(randomDouble(-1, 1));
|
||||
dx[i].setY(randomDouble(-1, 1));
|
||||
dx[i].setZ(randomDouble(-1, 1));
|
||||
}
|
||||
|
||||
btAlignedObjectArray<double> errors;
|
||||
for (int it = 0; it < 10; ++it)
|
||||
{
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i] *= 0.5;
|
||||
}
|
||||
|
||||
// get dphi/dx * dx
|
||||
double dphi = 0;
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dphi += dphi_dx[i].dot(dx[i]);
|
||||
}
|
||||
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btAlignedObjectArray<btSoftBody*> m_softBodies;
|
||||
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
|
||||
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] + dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double f1 = totalElasticEnergy(0);
|
||||
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] - dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
double f2 = totalElasticEnergy(0);
|
||||
|
||||
//restore m_q
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double error = f1-f2-2*dphi;
|
||||
errors.push_back(error);
|
||||
std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
|
||||
}
|
||||
for (int i = 1; i < errors.size(); ++i)
|
||||
{
|
||||
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testHessian()
|
||||
{
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
|
||||
|
||||
TVStack dx;
|
||||
dx.resize(getNumNodes());
|
||||
TVStack df;
|
||||
df.resize(dx.size());
|
||||
TVStack f1;
|
||||
f1.resize(dx.size());
|
||||
TVStack f2;
|
||||
f2.resize(dx.size());
|
||||
|
||||
|
||||
// write down the current position
|
||||
TVStack x;
|
||||
x.resize(dx.size());
|
||||
int counter = 0;
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
x[counter] = psb->m_nodes[j].m_q;
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
// populate dx with random vectors
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i].setX(randomDouble(-1, 1));
|
||||
dx[i].setY(randomDouble(-1, 1));
|
||||
dx[i].setZ(randomDouble(-1, 1));
|
||||
}
|
||||
|
||||
btAlignedObjectArray<double> errors;
|
||||
for (int it = 0; it < 10; ++it)
|
||||
{
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i] *= 0.5;
|
||||
}
|
||||
|
||||
// get df
|
||||
for (int i =0; i < df.size();++i)
|
||||
{
|
||||
df[i].setZero();
|
||||
f1[i].setZero();
|
||||
f2[i].setZero();
|
||||
}
|
||||
btDeformableLagrangianForce()
|
||||
{
|
||||
}
|
||||
|
||||
//set df
|
||||
addScaledElasticForceDifferential(-1, dx, df);
|
||||
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] + dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
//set f1
|
||||
addScaledForces(-1, f1);
|
||||
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] - dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
//set f2
|
||||
addScaledForces(-1, f2);
|
||||
|
||||
//restore m_q
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double error = 0;
|
||||
for (int i = 0; i < df.size();++i)
|
||||
{
|
||||
btVector3 error_vector = f1[i]-f2[i]-2*df[i];
|
||||
error += error_vector.length2();
|
||||
}
|
||||
error = btSqrt(error);
|
||||
errors.push_back(error);
|
||||
std::cout << "Iteration = " << it << ", error = " << error << std::endl;
|
||||
}
|
||||
for (int i = 1; i < errors.size(); ++i)
|
||||
{
|
||||
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
//
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// total Energy takes dt as input because certain energies depend on dt
|
||||
virtual double totalEnergy(btScalar dt)
|
||||
{
|
||||
return totalElasticEnergy(dt) + totalDampingEnergy(dt);
|
||||
}
|
||||
virtual ~btDeformableLagrangianForce() {}
|
||||
|
||||
// add all forces
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
// add damping df
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
|
||||
|
||||
// build diagonal of A matrix
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
|
||||
|
||||
// add elastic df
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
|
||||
|
||||
// add all forces that are explicit in explicit solve
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
// add all damping forces
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
|
||||
|
||||
virtual void addScaledHessian(btScalar scale) {}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType() = 0;
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
// get number of nodes that have the force
|
||||
virtual int getNumNodes()
|
||||
{
|
||||
int numNodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
numNodes += m_softBodies[i]->m_nodes.size();
|
||||
}
|
||||
return numNodes;
|
||||
}
|
||||
|
||||
// add a soft body to be affected by the particular lagrangian force
|
||||
virtual void addSoftBody(btSoftBody* psb)
|
||||
{
|
||||
m_softBodies.push_back(psb);
|
||||
}
|
||||
|
||||
virtual void removeSoftBody(btSoftBody* psb)
|
||||
{
|
||||
m_softBodies.remove(psb);
|
||||
}
|
||||
|
||||
virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
|
||||
{
|
||||
m_nodes = nodes;
|
||||
}
|
||||
|
||||
// Calculate the incremental deformable generated from the input dx
|
||||
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
|
||||
{
|
||||
btVector3 c1 = dx[id1] - dx[id0];
|
||||
btVector3 c2 = dx[id2] - dx[id0];
|
||||
btVector3 c3 = dx[id3] - dx[id0];
|
||||
return btMatrix3x3(c1, c2, c3).transpose();
|
||||
}
|
||||
|
||||
// Calculate the incremental deformable generated from the current velocity
|
||||
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
|
||||
{
|
||||
btVector3 c1 = n1->m_v - n0->m_v;
|
||||
btVector3 c2 = n2->m_v - n0->m_v;
|
||||
btVector3 c3 = n3->m_v - n0->m_v;
|
||||
return btMatrix3x3(c1, c2, c3).transpose();
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testDerivative()
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
|
||||
TVStack dx;
|
||||
dx.resize(getNumNodes());
|
||||
TVStack dphi_dx;
|
||||
dphi_dx.resize(dx.size());
|
||||
for (int i = 0; i < dphi_dx.size(); ++i)
|
||||
{
|
||||
dphi_dx[i].setZero();
|
||||
}
|
||||
addScaledForces(-1, dphi_dx);
|
||||
|
||||
// write down the current position
|
||||
TVStack x;
|
||||
x.resize(dx.size());
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
x[counter] = psb->m_nodes[j].m_q;
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
// populate dx with random vectors
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i].setX(randomDouble(-1, 1));
|
||||
dx[i].setY(randomDouble(-1, 1));
|
||||
dx[i].setZ(randomDouble(-1, 1));
|
||||
}
|
||||
|
||||
btAlignedObjectArray<double> errors;
|
||||
for (int it = 0; it < 10; ++it)
|
||||
{
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i] *= 0.5;
|
||||
}
|
||||
|
||||
// get dphi/dx * dx
|
||||
double dphi = 0;
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dphi += dphi_dx[i].dot(dx[i]);
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] + dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double f1 = totalElasticEnergy(0);
|
||||
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] - dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
double f2 = totalElasticEnergy(0);
|
||||
|
||||
//restore m_q
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double error = f1 - f2 - 2 * dphi;
|
||||
errors.push_back(error);
|
||||
std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
|
||||
}
|
||||
for (int i = 1; i < errors.size(); ++i)
|
||||
{
|
||||
std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testHessian()
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
|
||||
TVStack dx;
|
||||
dx.resize(getNumNodes());
|
||||
TVStack df;
|
||||
df.resize(dx.size());
|
||||
TVStack f1;
|
||||
f1.resize(dx.size());
|
||||
TVStack f2;
|
||||
f2.resize(dx.size());
|
||||
|
||||
// write down the current position
|
||||
TVStack x;
|
||||
x.resize(dx.size());
|
||||
int counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
x[counter] = psb->m_nodes[j].m_q;
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
// populate dx with random vectors
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i].setX(randomDouble(-1, 1));
|
||||
dx[i].setY(randomDouble(-1, 1));
|
||||
dx[i].setZ(randomDouble(-1, 1));
|
||||
}
|
||||
|
||||
btAlignedObjectArray<double> errors;
|
||||
for (int it = 0; it < 10; ++it)
|
||||
{
|
||||
for (int i = 0; i < dx.size(); ++i)
|
||||
{
|
||||
dx[i] *= 0.5;
|
||||
}
|
||||
|
||||
// get df
|
||||
for (int i = 0; i < df.size(); ++i)
|
||||
{
|
||||
df[i].setZero();
|
||||
f1[i].setZero();
|
||||
f2[i].setZero();
|
||||
}
|
||||
|
||||
//set df
|
||||
addScaledElasticForceDifferential(-1, dx, df);
|
||||
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] + dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
//set f1
|
||||
addScaledForces(-1, f1);
|
||||
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter] - dx[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
|
||||
//set f2
|
||||
addScaledForces(-1, f2);
|
||||
|
||||
//restore m_q
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
psb->m_nodes[j].m_q = x[counter];
|
||||
counter++;
|
||||
}
|
||||
psb->updateDeformation();
|
||||
}
|
||||
counter = 0;
|
||||
double error = 0;
|
||||
for (int i = 0; i < df.size(); ++i)
|
||||
{
|
||||
btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
|
||||
error += error_vector.length2();
|
||||
}
|
||||
error = btSqrt(error);
|
||||
errors.push_back(error);
|
||||
std::cout << "Iteration = " << it << ", error = " << error << std::endl;
|
||||
}
|
||||
for (int i = 1; i < errors.size(); ++i)
|
||||
{
|
||||
std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
//
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// total Energy takes dt as input because certain energies depend on dt
|
||||
virtual double totalEnergy(btScalar dt)
|
||||
{
|
||||
return totalElasticEnergy(dt) + totalDampingEnergy(dt);
|
||||
}
|
||||
};
|
||||
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
|
||||
|
@ -18,323 +18,445 @@
|
||||
|
||||
#include "btDeformableLagrangianForce.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSoftBodyInternals.h"
|
||||
#define TETRA_FLAT_THRESHOLD 0.01
|
||||
class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda;
|
||||
btScalar m_mu_damp, m_lambda_damp;
|
||||
btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1)
|
||||
{
|
||||
btScalar damping = 0.05;
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
}
|
||||
|
||||
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
|
||||
{
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * df_on_node0;
|
||||
force[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
|
||||
energy += tetra.m_element_measure * elasticEnergyDensity(s);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz+1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
|
||||
{
|
||||
double density = 0;
|
||||
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
|
||||
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
|
||||
density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
|
||||
density += m_lambda * trace * trace * 0.5;
|
||||
return density;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar max_p = psb->m_cfg.m_maxStress;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j],P);
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda;
|
||||
btScalar m_E, m_nu; // Young's modulus and Poisson ratio
|
||||
btScalar m_damping_alpha, m_damping_beta;
|
||||
btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01)
|
||||
{
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
|
||||
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta)
|
||||
{
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
|
||||
void updateYoungsModulusAndPoissonRatio()
|
||||
{
|
||||
// conversion from Lame Parameters to Young's modulus and Poisson ratio
|
||||
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
|
||||
m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
|
||||
m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
|
||||
}
|
||||
|
||||
void updateLameParameters()
|
||||
{
|
||||
// conversion from Young's modulus and Poisson ratio to Lame Parameters
|
||||
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
|
||||
m_mu = m_E * 0.5 / (1 + m_nu);
|
||||
m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
|
||||
}
|
||||
|
||||
void setYoungsModulus(btScalar E)
|
||||
{
|
||||
m_E = E;
|
||||
updateLameParameters();
|
||||
}
|
||||
|
||||
void setPoissonRatio(btScalar nu)
|
||||
{
|
||||
m_nu = nu;
|
||||
updateLameParameters();
|
||||
}
|
||||
|
||||
void setDamping(btScalar damping_alpha, btScalar damping_beta)
|
||||
{
|
||||
m_damping_alpha = damping_alpha;
|
||||
m_damping_beta = damping_beta;
|
||||
}
|
||||
|
||||
void setLameParameters(btScalar mu, btScalar lambda)
|
||||
{
|
||||
m_mu = mu;
|
||||
m_lambda = lambda;
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
if (m_damping_alpha == 0 && m_damping_beta == 0)
|
||||
return;
|
||||
btScalar mu_damp = m_damping_beta * m_mu;
|
||||
btScalar lambda_damp = m_damping_beta * m_lambda;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
|
||||
if (!close_to_flat)
|
||||
{
|
||||
dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
|
||||
}
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
if (!close_to_flat)
|
||||
{
|
||||
df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
|
||||
}
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * df_on_node0;
|
||||
force[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
size_t id = node.index;
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
force[id] -= scale * node.m_v / node.m_im * m_damping_alpha;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
|
||||
energy += tetra.m_element_measure * elasticEnergyDensity(s);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz + 1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
|
||||
{
|
||||
double density = 0;
|
||||
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
|
||||
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
|
||||
density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
|
||||
density += m_lambda * trace * trace * 0.5;
|
||||
return density;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar max_p = psb->m_cfg.m_maxStress;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j], P);
|
||||
#if USE_SVD
|
||||
if (max_p > 0)
|
||||
{
|
||||
// since we want to clamp the principal stress to max_p, we only need to
|
||||
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
|
||||
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
|
||||
if (trPTP > max_p * max_p)
|
||||
{
|
||||
btMatrix3x3 U, V;
|
||||
btVector3 sigma;
|
||||
singularValueDecomposition(P, U, sigma, V);
|
||||
sigma[0] = btMin(sigma[0], max_p);
|
||||
sigma[1] = btMin(sigma[1], max_p);
|
||||
sigma[2] = btMin(sigma[2], max_p);
|
||||
sigma[0] = btMax(sigma[0], -max_p);
|
||||
sigma[1] = btMax(sigma[1], -max_p);
|
||||
sigma[2] = btMax(sigma[2], -max_p);
|
||||
btMatrix3x3 Sigma;
|
||||
Sigma.setIdentity();
|
||||
Sigma[0][0] = sigma[0];
|
||||
Sigma[1][1] = sigma[1];
|
||||
Sigma[2][2] = sigma[2];
|
||||
P = U * Sigma * V.transpose();
|
||||
}
|
||||
}
|
||||
if (max_p > 0)
|
||||
{
|
||||
// since we want to clamp the principal stress to max_p, we only need to
|
||||
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
|
||||
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
|
||||
if (trPTP > max_p * max_p)
|
||||
{
|
||||
btMatrix3x3 U, V;
|
||||
btVector3 sigma;
|
||||
singularValueDecomposition(P, U, sigma, V);
|
||||
sigma[0] = btMin(sigma[0], max_p);
|
||||
sigma[1] = btMin(sigma[1], max_p);
|
||||
sigma[2] = btMin(sigma[2], max_p);
|
||||
sigma[0] = btMax(sigma[0], -max_p);
|
||||
sigma[1] = btMax(sigma[1], -max_p);
|
||||
sigma[2] = btMax(sigma[2], -max_p);
|
||||
btMatrix3x3 Sigma;
|
||||
Sigma.setIdentity();
|
||||
Sigma[0][0] = sigma[0];
|
||||
Sigma[1][1] = sigma[1];
|
||||
Sigma[2][2] = sigma[2];
|
||||
P = U * Sigma * V.transpose();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 dP;
|
||||
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// elastic force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
||||
{
|
||||
btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
|
||||
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
|
||||
P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
|
||||
}
|
||||
|
||||
// Let P be the first piola stress.
|
||||
// This function calculates the dP = dP/dF * dF
|
||||
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
|
||||
dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
|
||||
}
|
||||
|
||||
// Let Q be the damping stress.
|
||||
// This function calculates the dP = dQ/dF * dF
|
||||
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
|
||||
dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace;
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_LINEAR_ELASTICITY_FORCE;
|
||||
}
|
||||
|
||||
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
if (m_damping_alpha == 0 && m_damping_beta == 0)
|
||||
return;
|
||||
btScalar mu_damp = m_damping_beta * m_mu;
|
||||
btScalar lambda_damp = m_damping_beta * m_lambda;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||
if (!close_to_flat)
|
||||
{
|
||||
dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
|
||||
}
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
if (!close_to_flat)
|
||||
{
|
||||
df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
|
||||
}
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
size_t id = node.index;
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
df[id] -= scale * dv[id] / node.m_im * m_damping_alpha;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 dP;
|
||||
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// elastic force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
||||
{
|
||||
btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F;
|
||||
|
||||
btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
|
||||
btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
|
||||
P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
|
||||
}
|
||||
|
||||
// Let P be the first piola stress.
|
||||
// This function calculates the dP = dP/dF * dF
|
||||
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
|
||||
dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
|
||||
}
|
||||
|
||||
// Let Q be the damping stress.
|
||||
// This function calculates the dP = dQ/dF * dF
|
||||
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar mu_damp = m_damping_beta * m_mu;
|
||||
btScalar lambda_damp = m_damping_beta * m_lambda;
|
||||
btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
|
||||
dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace;
|
||||
}
|
||||
|
||||
virtual void addScaledHessian(btScalar scale)
|
||||
{
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn
|
||||
btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms;
|
||||
node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1;
|
||||
node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1;
|
||||
node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1;
|
||||
node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
btSoftBody::Node& node = psb->m_nodes[j];
|
||||
if (node.m_im > 0)
|
||||
{
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_LINEAR_ELASTICITY_FORCE;
|
||||
}
|
||||
};
|
||||
#endif /* BT_LINEAR_ELASTICITY_H */
|
||||
|
@ -20,282 +20,282 @@
|
||||
|
||||
class btDeformableMassSpringForce : public btDeformableLagrangianForce
|
||||
{
|
||||
// If true, the damping force will be in the direction of the spring
|
||||
// If false, the damping force will be in the direction of the velocity
|
||||
bool m_momentum_conserving;
|
||||
btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
|
||||
// If true, the damping force will be in the direction of the spring
|
||||
// If false, the damping force will be in the direction of the velocity
|
||||
bool m_momentum_conserving;
|
||||
btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
|
||||
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
|
||||
{
|
||||
}
|
||||
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
|
||||
{
|
||||
if (m_bendingStiffness < btScalar(0))
|
||||
{
|
||||
m_bendingStiffness = m_elasticStiffness;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
|
||||
// damping force
|
||||
btVector3 v_diff = (node2->m_v - node1->m_v);
|
||||
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
}
|
||||
force[id1] += scaled_force;
|
||||
force[id2] -= scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
btScalar r = link.m_rl;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
|
||||
// elastic force
|
||||
btVector3 dir = (node2->m_q - node1->m_q);
|
||||
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
|
||||
btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
|
||||
btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
|
||||
force[id1] += scaled_force;
|
||||
force[id2] -= scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
|
||||
{
|
||||
}
|
||||
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
|
||||
{
|
||||
if (m_bendingStiffness < btScalar(0))
|
||||
{
|
||||
m_bendingStiffness = m_elasticStiffness;
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
|
||||
}
|
||||
}
|
||||
df[id1] += local_scaled_df;
|
||||
df[id2] -= local_scaled_df;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
if (node1->m_im > 0)
|
||||
diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
|
||||
if (node2->m_im > 0)
|
||||
diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
if (node1->m_im > 0)
|
||||
diagA[id1][d] -= scaled_k_damp;
|
||||
if (node2->m_im > 0)
|
||||
diagA[id2][d] -= scaled_k_damp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
btScalar r = link.m_rl;
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// elastic force
|
||||
btVector3 dir = (node2->m_q - node1->m_q);
|
||||
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz+1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
btScalar r = link.m_rl;
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
btVector3 dir = (node1->m_q - node2->m_q);
|
||||
btScalar dir_norm = dir.norm();
|
||||
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
|
||||
btVector3 dx_diff = dx[id1] - dx[id2];
|
||||
btVector3 scaled_df = btVector3(0,0,0);
|
||||
btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
|
||||
if (dir_norm > SIMD_EPSILON)
|
||||
{
|
||||
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
|
||||
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
|
||||
scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
|
||||
}
|
||||
|
||||
df[id1] += scaled_df;
|
||||
df[id2] -= scaled_df;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_MASSSPRING_FORCE;
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
|
||||
// damping force
|
||||
btVector3 v_diff = (node2->m_v - node1->m_v);
|
||||
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
}
|
||||
force[id1] += scaled_force;
|
||||
force[id2] -= scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
btScalar r = link.m_rl;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
|
||||
// elastic force
|
||||
btVector3 dir = (node2->m_q - node1->m_q);
|
||||
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
|
||||
btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
|
||||
btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
|
||||
force[id1] += scaled_force;
|
||||
force[id2] -= scaled_force;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
|
||||
btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
|
||||
}
|
||||
}
|
||||
df[id1] += local_scaled_df;
|
||||
df[id2] -= local_scaled_df;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
if (m_momentum_conserving)
|
||||
{
|
||||
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (node2->m_x - node1->m_x).normalized();
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
if (node1->m_im > 0)
|
||||
diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
|
||||
if (node2->m_im > 0)
|
||||
diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
if (node1->m_im > 0)
|
||||
diagA[id1][d] -= scaled_k_damp;
|
||||
if (node2->m_im > 0)
|
||||
diagA[id2][d] -= scaled_k_damp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
btScalar r = link.m_rl;
|
||||
|
||||
// elastic force
|
||||
btVector3 dir = (node2->m_q - node1->m_q);
|
||||
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz + 1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
// implicit damping force differential
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
const btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_links.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Link& link = psb->m_links[j];
|
||||
btSoftBody::Node* node1 = link.m_n[0];
|
||||
btSoftBody::Node* node2 = link.m_n[1];
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
btScalar r = link.m_rl;
|
||||
|
||||
btVector3 dir = (node1->m_q - node2->m_q);
|
||||
btScalar dir_norm = dir.norm();
|
||||
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
|
||||
btVector3 dx_diff = dx[id1] - dx[id2];
|
||||
btVector3 scaled_df = btVector3(0, 0, 0);
|
||||
btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
|
||||
if (dir_norm > SIMD_EPSILON)
|
||||
{
|
||||
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
|
||||
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
|
||||
scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
|
||||
}
|
||||
|
||||
df[id1] += scaled_df;
|
||||
df[id2] -= scaled_df;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_MASSSPRING_FORCE;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* btMassSpring_h */
|
||||
|
@ -20,126 +20,143 @@
|
||||
|
||||
class btDeformableMousePickingForce : public btDeformableLagrangianForce
|
||||
{
|
||||
// If true, the damping force will be in the direction of the spring
|
||||
// If false, the damping force will be in the direction of the velocity
|
||||
btScalar m_elasticStiffness, m_dampingStiffness;
|
||||
const btSoftBody::Face& m_face;
|
||||
btVector3 m_mouse_pos;
|
||||
btScalar m_maxForce;
|
||||
// If true, the damping force will be in the direction of the spring
|
||||
// If false, the damping force will be in the direction of the velocity
|
||||
btScalar m_elasticStiffness, m_dampingStiffness;
|
||||
const btSoftBody::Face& m_face;
|
||||
btVector3 m_mouse_pos;
|
||||
btScalar m_maxForce;
|
||||
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 v_diff = m_face.m_n[i]->m_v;
|
||||
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
force[m_face.m_n[i]->index] -= scaled_force;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
btScalar scaled_stiffness = scale * m_elasticStiffness;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
|
||||
btVector3 scaled_force = scaled_stiffness * dir;
|
||||
if (scaled_force.safeNorm() > m_maxForce)
|
||||
{
|
||||
scaled_force.safeNormalize();
|
||||
scaled_force *= m_maxForce;
|
||||
}
|
||||
force[m_face.m_n[i]->index] -= scaled_force;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
|
||||
}
|
||||
df[m_face.m_n[i]->index] -= local_scaled_df;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
|
||||
btVector3 scaled_force = m_elasticStiffness * dir;
|
||||
if (scaled_force.safeNorm() > m_maxForce)
|
||||
{
|
||||
scaled_force.safeNormalize();
|
||||
scaled_force *= m_maxForce;
|
||||
}
|
||||
energy += 0.5 * scaled_force.dot(dir);
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 v_diff = m_face.m_n[i]->m_v;
|
||||
btVector3 scaled_force = m_dampingStiffness * v_diff;
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
//TODO
|
||||
}
|
||||
|
||||
void setMousePos(const btVector3& p)
|
||||
{
|
||||
m_mouse_pos = p;
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_MOUSE_PICKING_FORCE;
|
||||
}
|
||||
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 v_diff = m_face.m_n[i]->m_v;
|
||||
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
force[m_face.m_n[i]->index] -= scaled_force;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
btScalar scaled_stiffness = scale * m_elasticStiffness;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
|
||||
btVector3 scaled_force = scaled_stiffness * dir;
|
||||
if (scaled_force.safeNorm() > m_maxForce)
|
||||
{
|
||||
scaled_force.safeNormalize();
|
||||
scaled_force *= m_maxForce;
|
||||
}
|
||||
force[m_face.m_n[i]->index] -= scaled_force;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
btScalar scaled_k_damp = m_dampingStiffness * scale;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
|
||||
}
|
||||
df[m_face.m_n[i]->index] -= local_scaled_df;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
|
||||
btVector3 scaled_force = m_elasticStiffness * dir;
|
||||
if (scaled_force.safeNorm() > m_maxForce)
|
||||
{
|
||||
scaled_force.safeNormalize();
|
||||
scaled_force *= m_maxForce;
|
||||
}
|
||||
energy += 0.5 * scaled_force.dot(dir);
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 v_diff = m_face.m_n[i]->m_v;
|
||||
btVector3 scaled_force = m_dampingStiffness * v_diff;
|
||||
if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
|
||||
scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
|
||||
}
|
||||
energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
btScalar scaled_stiffness = scale * m_elasticStiffness;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
|
||||
btScalar dir_norm = dir.norm();
|
||||
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
|
||||
int id = m_face.m_n[i]->index;
|
||||
btVector3 dx_diff = dx[id];
|
||||
btScalar r = 0; // rest length is 0 for picking spring
|
||||
btVector3 scaled_df = btVector3(0, 0, 0);
|
||||
if (dir_norm > SIMD_EPSILON)
|
||||
{
|
||||
scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized;
|
||||
scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
|
||||
scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff;
|
||||
}
|
||||
df[id] += scaled_df;
|
||||
}
|
||||
}
|
||||
|
||||
void setMousePos(const btVector3& p)
|
||||
{
|
||||
m_mouse_pos = p;
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_MOUSE_PICKING_FORCE;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* btMassSpring_h */
|
||||
|
@ -13,131 +13,132 @@
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btDeformableMultiBodyConstraintSolver.h"
|
||||
#include <iostream>
|
||||
// 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)
|
||||
btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
|
||||
{
|
||||
{
|
||||
///this is a special step to resolve penetrations (just for contacts)
|
||||
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
{
|
||||
///this is a special step to resolve penetrations (just for contacts)
|
||||
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
|
||||
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
|
||||
for (int iteration = 0; iteration < maxIterations; iteration++)
|
||||
{
|
||||
// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
|
||||
|
||||
// solve rigid/rigid in solver body
|
||||
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
// solver body velocity -> rigid body velocity
|
||||
solverBodyWriteBack(infoGlobal);
|
||||
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
|
||||
// update rigid body velocity in rigid/deformable contact
|
||||
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
|
||||
// solver body velocity <- rigid body velocity
|
||||
writeToSolverBody(bodies, numBodies, infoGlobal);
|
||||
|
||||
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
|
||||
{
|
||||
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
|
||||
for (int iteration = 0; iteration < maxIterations; iteration++)
|
||||
{
|
||||
// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
|
||||
|
||||
// solve rigid/rigid in solver body
|
||||
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
// solver body velocity -> rigid body velocity
|
||||
solverBodyWriteBack(infoGlobal);
|
||||
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
|
||||
// update rigid body velocity in rigid/deformable contact
|
||||
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
|
||||
// solver body velocity <- rigid body velocity
|
||||
writeToSolverBody(bodies, numBodies, infoGlobal);
|
||||
|
||||
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
|
||||
{
|
||||
#ifdef VERBOSE_RESIDUAL_PRINTF
|
||||
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
|
||||
if (iteration >= (maxIterations - 1))
|
||||
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
|
||||
#endif
|
||||
m_analyticsData.m_numSolverCalls++;
|
||||
m_analyticsData.m_numIterationsUsed = iteration+1;
|
||||
m_analyticsData.m_islandId = -2;
|
||||
if (numBodies>0)
|
||||
m_analyticsData.m_islandId = bodies[0]->getCompanionId();
|
||||
m_analyticsData.m_numBodies = numBodies;
|
||||
m_analyticsData.m_numContactManifolds = numManifolds;
|
||||
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0.f;
|
||||
m_analyticsData.m_numSolverCalls++;
|
||||
m_analyticsData.m_numIterationsUsed = iteration + 1;
|
||||
m_analyticsData.m_islandId = -2;
|
||||
if (numBodies > 0)
|
||||
m_analyticsData.m_islandId = bodies[0]->getCompanionId();
|
||||
m_analyticsData.m_numBodies = numBodies;
|
||||
m_analyticsData.m_numContactManifolds = numManifolds;
|
||||
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
|
||||
void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
|
||||
{
|
||||
m_tmpMultiBodyConstraints = multiBodyConstraints;
|
||||
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
|
||||
|
||||
// inherited from MultiBodyConstraintSolver
|
||||
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||
|
||||
// overriden
|
||||
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||
|
||||
// inherited from MultiBodyConstraintSolver
|
||||
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
|
||||
|
||||
m_tmpMultiBodyConstraints = 0;
|
||||
m_tmpNumMultiBodyConstraints = 0;
|
||||
m_tmpMultiBodyConstraints = multiBodyConstraints;
|
||||
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
|
||||
|
||||
// inherited from MultiBodyConstraintSolver
|
||||
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||
|
||||
// overriden
|
||||
solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||
|
||||
// inherited from MultiBodyConstraintSolver
|
||||
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
|
||||
|
||||
m_tmpMultiBodyConstraints = 0;
|
||||
m_tmpNumMultiBodyConstraints = 0;
|
||||
}
|
||||
|
||||
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
||||
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
|
||||
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
|
||||
}
|
||||
}
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
||||
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
|
||||
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
|
||||
{
|
||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
if (body)
|
||||
{
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
|
||||
{
|
||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
if (body)
|
||||
{
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
|
||||
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");
|
||||
int iteration;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
{
|
||||
// m_deformableSolver->splitImpulseSetup(infoGlobal);
|
||||
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
|
||||
{
|
||||
btScalar leastSquaresResidual = 0.f;
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int j;
|
||||
for (j = 0; j < numPoolConstraints; j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
|
||||
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
// solve the position correction between deformable and rigid/multibody
|
||||
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
|
||||
// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
|
||||
int iteration;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
{
|
||||
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
|
||||
{
|
||||
btScalar leastSquaresResidual = 0.f;
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int j;
|
||||
for (j = 0; j < numPoolConstraints; j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
|
||||
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
|
||||
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
// solve the position correction between deformable and rigid/multibody
|
||||
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
|
||||
btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
|
||||
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
|
||||
}
|
||||
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
|
||||
{
|
||||
#ifdef VERBOSE_RESIDUAL_PRINTF
|
||||
printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
|
||||
if (iteration >= (infoGlobal.m_numIterations - 1))
|
||||
printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -13,7 +13,6 @@
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
@ -32,30 +31,31 @@ class btDeformableBodySolver;
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
|
||||
{
|
||||
btDeformableBodySolver* m_deformableSolver;
|
||||
|
||||
protected:
|
||||
// override the iterations method to include deformable/multibody contact
|
||||
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
// write the velocity of the the solver body to the underlying rigid body
|
||||
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
|
||||
btDeformableBodySolver* m_deformableSolver;
|
||||
|
||||
protected:
|
||||
// override the iterations method to include deformable/multibody contact
|
||||
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
// write the velocity of the the solver body to the underlying rigid body
|
||||
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
// write the velocity of the underlying rigid body to the the the solver body
|
||||
void writeToSolverBody(btCollisionObject * *bodies, int numBodies, 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);
|
||||
|
||||
// write the velocity of the underlying rigid body to the the the solver body
|
||||
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, 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);
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
void setDeformableSolver(btDeformableBodySolver* deformableSolver)
|
||||
{
|
||||
m_deformableSolver = deformableSolver;
|
||||
}
|
||||
|
||||
virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
void setDeformableSolver(btDeformableBodySolver * deformableSolver)
|
||||
{
|
||||
m_deformableSolver = deformableSolver;
|
||||
}
|
||||
|
||||
virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
|
||||
};
|
||||
|
||||
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -36,185 +36,192 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
|
||||
|
||||
class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
|
||||
{
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
///Solver classes that encapsulate multiple deformable bodies for solving
|
||||
btDeformableBodySolver* m_deformableBodySolver;
|
||||
btSoftBodyArray m_softBodies;
|
||||
int m_drawFlags;
|
||||
bool m_drawNodeTree;
|
||||
bool m_drawFaceTree;
|
||||
bool m_drawClusterTree;
|
||||
btSoftBodyWorldInfo m_sbi;
|
||||
btScalar m_internalTime;
|
||||
int m_ccdIterations;
|
||||
bool m_implicit;
|
||||
bool m_lineSearch;
|
||||
bool m_useProjection;
|
||||
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
|
||||
|
||||
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
|
||||
btSolverCallback m_solverCallback;
|
||||
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
///Solver classes that encapsulate multiple deformable bodies for solving
|
||||
btDeformableBodySolver* m_deformableBodySolver;
|
||||
btSoftBodyArray m_softBodies;
|
||||
int m_drawFlags;
|
||||
bool m_drawNodeTree;
|
||||
bool m_drawFaceTree;
|
||||
bool m_drawClusterTree;
|
||||
btSoftBodyWorldInfo m_sbi;
|
||||
btScalar m_internalTime;
|
||||
int m_ccdIterations;
|
||||
bool m_implicit;
|
||||
bool m_lineSearch;
|
||||
bool m_useProjection;
|
||||
DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
|
||||
|
||||
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
|
||||
btSolverCallback m_solverCallback;
|
||||
|
||||
protected:
|
||||
virtual void internalSingleStepSimulation(btScalar timeStep);
|
||||
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
|
||||
void positionCorrection(btScalar timeStep);
|
||||
|
||||
void solveConstraints(btScalar timeStep);
|
||||
|
||||
void updateActivationState(btScalar timeStep);
|
||||
|
||||
void clearGravity();
|
||||
|
||||
virtual void internalSingleStepSimulation(btScalar timeStep);
|
||||
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
|
||||
void positionCorrection(btScalar timeStep);
|
||||
|
||||
void solveConstraints(btScalar timeStep);
|
||||
|
||||
void updateActivationState(btScalar timeStep);
|
||||
|
||||
void clearGravity();
|
||||
|
||||
public:
|
||||
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
|
||||
|
||||
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
|
||||
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
|
||||
|
||||
virtual void debugDrawWorld();
|
||||
|
||||
void setSolverCallback(btSolverCallback cb)
|
||||
{
|
||||
m_solverCallback = cb;
|
||||
}
|
||||
|
||||
virtual ~btDeformableMultiBodyDynamicsWorld();
|
||||
|
||||
virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
|
||||
{
|
||||
return (btMultiBodyDynamicsWorld*)(this);
|
||||
}
|
||||
|
||||
virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
|
||||
{
|
||||
return (const btMultiBodyDynamicsWorld*)(this);
|
||||
}
|
||||
|
||||
virtual btDynamicsWorldType getWorldType() const
|
||||
{
|
||||
return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
|
||||
}
|
||||
|
||||
virtual void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
|
||||
|
||||
btSoftBodyArray& getSoftBodyArray()
|
||||
{
|
||||
return m_softBodies;
|
||||
}
|
||||
|
||||
const btSoftBodyArray& getSoftBodyArray() const
|
||||
{
|
||||
return m_softBodies;
|
||||
}
|
||||
|
||||
btSoftBodyWorldInfo& getWorldInfo()
|
||||
{
|
||||
return m_sbi;
|
||||
}
|
||||
|
||||
const btSoftBodyWorldInfo& getWorldInfo() const
|
||||
{
|
||||
return m_sbi;
|
||||
}
|
||||
|
||||
void reinitialize(btScalar timeStep);
|
||||
|
||||
void applyRigidBodyGravity(btScalar timeStep);
|
||||
|
||||
void beforeSolverCallbacks(btScalar timeStep);
|
||||
|
||||
void afterSolverCallbacks(btScalar timeStep);
|
||||
|
||||
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
|
||||
|
||||
void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
|
||||
|
||||
void removeSoftBody(btSoftBody* body);
|
||||
|
||||
void removeCollisionObject(btCollisionObject* collisionObject);
|
||||
|
||||
int getDrawFlags() const { return (m_drawFlags); }
|
||||
void setDrawFlags(int f) { m_drawFlags = f; }
|
||||
|
||||
void setupConstraints();
|
||||
|
||||
void performDeformableCollisionDetection();
|
||||
|
||||
void solveMultiBodyConstraints();
|
||||
|
||||
void solveContactConstraints();
|
||||
|
||||
void sortConstraints();
|
||||
|
||||
void softBodySelfCollision();
|
||||
|
||||
void setImplicit(bool implicit)
|
||||
{
|
||||
m_implicit = implicit;
|
||||
}
|
||||
|
||||
void setLineSearch(bool lineSearch)
|
||||
{
|
||||
m_lineSearch = lineSearch;
|
||||
}
|
||||
|
||||
void applyRepulsionForce(btScalar timeStep);
|
||||
|
||||
void performGeometricCollisions(btScalar timeStep);
|
||||
|
||||
struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
|
||||
{
|
||||
btVector3 m_rayFromWorld;
|
||||
btVector3 m_rayToWorld;
|
||||
btTransform m_rayFromTrans;
|
||||
btTransform m_rayToTrans;
|
||||
btVector3 m_hitNormal;
|
||||
|
||||
const btDeformableMultiBodyDynamicsWorld* m_world;
|
||||
btCollisionWorld::RayResultCallback& m_resultCallback;
|
||||
|
||||
btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
|
||||
: m_rayFromWorld(rayFromWorld),
|
||||
m_rayToWorld(rayToWorld),
|
||||
m_world(world),
|
||||
m_resultCallback(resultCallback)
|
||||
{
|
||||
m_rayFromTrans.setIdentity();
|
||||
m_rayFromTrans.setOrigin(m_rayFromWorld);
|
||||
m_rayToTrans.setIdentity();
|
||||
m_rayToTrans.setOrigin(m_rayToWorld);
|
||||
|
||||
btVector3 rayDir = (rayToWorld - rayFromWorld);
|
||||
|
||||
rayDir.normalize();
|
||||
///what about division by zero? --> just set rayDirection[i] to INF/1e30
|
||||
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
|
||||
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
|
||||
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
|
||||
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
|
||||
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
|
||||
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
|
||||
|
||||
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
|
||||
}
|
||||
|
||||
virtual bool process(const btBroadphaseProxy* proxy)
|
||||
{
|
||||
///terminate further ray tests, once the closestHitFraction reached zero
|
||||
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
|
||||
return false;
|
||||
|
||||
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
|
||||
|
||||
//only perform raycast if filterMask matches
|
||||
if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
|
||||
{
|
||||
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
|
||||
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
|
||||
void setSolverCallback(btSolverCallback cb)
|
||||
{
|
||||
m_solverCallback = cb;
|
||||
}
|
||||
|
||||
virtual ~btDeformableMultiBodyDynamicsWorld();
|
||||
|
||||
virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
|
||||
{
|
||||
return (btMultiBodyDynamicsWorld*)(this);
|
||||
}
|
||||
|
||||
virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
|
||||
{
|
||||
return (const btMultiBodyDynamicsWorld*)(this);
|
||||
}
|
||||
|
||||
virtual btDynamicsWorldType getWorldType() const
|
||||
{
|
||||
return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
|
||||
}
|
||||
|
||||
virtual void predictUnconstraintMotion(btScalar timeStep);
|
||||
|
||||
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
|
||||
|
||||
btSoftBodyArray& getSoftBodyArray()
|
||||
{
|
||||
return m_softBodies;
|
||||
}
|
||||
|
||||
const btSoftBodyArray& getSoftBodyArray() const
|
||||
{
|
||||
return m_softBodies;
|
||||
}
|
||||
|
||||
btSoftBodyWorldInfo& getWorldInfo()
|
||||
{
|
||||
return m_sbi;
|
||||
}
|
||||
|
||||
const btSoftBodyWorldInfo& getWorldInfo() const
|
||||
{
|
||||
return m_sbi;
|
||||
}
|
||||
|
||||
void reinitialize(btScalar timeStep);
|
||||
|
||||
void applyRigidBodyGravity(btScalar timeStep);
|
||||
|
||||
void beforeSolverCallbacks(btScalar timeStep);
|
||||
|
||||
void afterSolverCallbacks(btScalar timeStep);
|
||||
|
||||
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
|
||||
|
||||
void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
|
||||
|
||||
void removeSoftBodyForce(btSoftBody* psb);
|
||||
|
||||
void removeSoftBody(btSoftBody* body);
|
||||
|
||||
void removeCollisionObject(btCollisionObject* collisionObject);
|
||||
|
||||
int getDrawFlags() const { return (m_drawFlags); }
|
||||
void setDrawFlags(int f) { m_drawFlags = f; }
|
||||
|
||||
void setupConstraints();
|
||||
|
||||
void performDeformableCollisionDetection();
|
||||
|
||||
void solveMultiBodyConstraints();
|
||||
|
||||
void solveContactConstraints();
|
||||
|
||||
void sortConstraints();
|
||||
|
||||
void softBodySelfCollision();
|
||||
|
||||
void setImplicit(bool implicit)
|
||||
{
|
||||
m_implicit = implicit;
|
||||
}
|
||||
|
||||
void setLineSearch(bool lineSearch)
|
||||
{
|
||||
m_lineSearch = lineSearch;
|
||||
}
|
||||
|
||||
void setUseProjection(bool useProjection)
|
||||
{
|
||||
m_useProjection = useProjection;
|
||||
}
|
||||
|
||||
void applyRepulsionForce(btScalar timeStep);
|
||||
|
||||
void performGeometricCollisions(btScalar timeStep);
|
||||
|
||||
struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
|
||||
{
|
||||
btVector3 m_rayFromWorld;
|
||||
btVector3 m_rayToWorld;
|
||||
btTransform m_rayFromTrans;
|
||||
btTransform m_rayToTrans;
|
||||
btVector3 m_hitNormal;
|
||||
|
||||
const btDeformableMultiBodyDynamicsWorld* m_world;
|
||||
btCollisionWorld::RayResultCallback& m_resultCallback;
|
||||
|
||||
btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
|
||||
: m_rayFromWorld(rayFromWorld),
|
||||
m_rayToWorld(rayToWorld),
|
||||
m_world(world),
|
||||
m_resultCallback(resultCallback)
|
||||
{
|
||||
m_rayFromTrans.setIdentity();
|
||||
m_rayFromTrans.setOrigin(m_rayFromWorld);
|
||||
m_rayToTrans.setIdentity();
|
||||
m_rayToTrans.setOrigin(m_rayToWorld);
|
||||
|
||||
btVector3 rayDir = (rayToWorld - rayFromWorld);
|
||||
|
||||
rayDir.normalize();
|
||||
///what about division by zero? --> just set rayDirection[i] to INF/1e30
|
||||
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
|
||||
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
|
||||
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
|
||||
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
|
||||
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
|
||||
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
|
||||
|
||||
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
|
||||
}
|
||||
|
||||
virtual bool process(const btBroadphaseProxy* proxy)
|
||||
{
|
||||
///terminate further ray tests, once the closestHitFraction reached zero
|
||||
if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
|
||||
return false;
|
||||
|
||||
btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
|
||||
|
||||
//only perform raycast if filterMask matches
|
||||
if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
|
||||
{
|
||||
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
|
||||
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
|
||||
#if 0
|
||||
#ifdef RECALCULATE_AABB
|
||||
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
|
||||
@ -225,87 +232,85 @@ public:
|
||||
const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
|
||||
#endif
|
||||
#endif
|
||||
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
|
||||
//culling already done by broadphase
|
||||
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
|
||||
{
|
||||
m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
|
||||
//culling already done by broadphase
|
||||
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
|
||||
{
|
||||
m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
|
||||
{
|
||||
BT_PROFILE("rayTest");
|
||||
/// use the broadphase to accelerate the search for objects, based on their aabb
|
||||
/// and for each object with ray-aabb overlap, perform an exact ray test
|
||||
btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
|
||||
|
||||
|
||||
|
||||
void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
|
||||
{
|
||||
BT_PROFILE("rayTest");
|
||||
/// use the broadphase to accelerate the search for objects, based on their aabb
|
||||
/// and for each object with ray-aabb overlap, perform an exact ray test
|
||||
btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
|
||||
|
||||
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
|
||||
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
|
||||
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
|
||||
#else
|
||||
for (int i = 0; i < this->getNumCollisionObjects(); i++)
|
||||
{
|
||||
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
|
||||
}
|
||||
for (int i = 0; i < this->getNumCollisionObjects(); i++)
|
||||
{
|
||||
rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
|
||||
}
|
||||
#endif //USE_BRUTEFORCE_RAYBROADPHASE
|
||||
}
|
||||
|
||||
void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
RayResultCallback& resultCallback) const
|
||||
{
|
||||
if (collisionShape->isSoftBody())
|
||||
{
|
||||
btSoftBody* softBody = btSoftBody::upcast(collisionObject);
|
||||
if (softBody)
|
||||
{
|
||||
btSoftBody::sRayCast softResult;
|
||||
if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
|
||||
{
|
||||
if (softResult.fraction <= resultCallback.m_closestHitFraction)
|
||||
{
|
||||
btCollisionWorld::LocalShapeInfo shapeInfo;
|
||||
shapeInfo.m_shapePart = 0;
|
||||
shapeInfo.m_triangleIndex = softResult.index;
|
||||
// get the normal
|
||||
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
|
||||
btVector3 normal = -rayDir;
|
||||
normal.normalize();
|
||||
{
|
||||
normal = softBody->m_faces[softResult.index].m_normal;
|
||||
if (normal.dot(rayDir) > 0)
|
||||
{
|
||||
// normal always point toward origin of the ray
|
||||
normal = -normal;
|
||||
}
|
||||
}
|
||||
|
||||
btCollisionWorld::LocalRayResult rayResult(collisionObject,
|
||||
&shapeInfo,
|
||||
normal,
|
||||
softResult.fraction);
|
||||
bool normalInWorldSpace = true;
|
||||
resultCallback.addSingleResult(rayResult, normalInWorldSpace);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
RayResultCallback& resultCallback) const
|
||||
{
|
||||
if (collisionShape->isSoftBody())
|
||||
{
|
||||
btSoftBody* softBody = btSoftBody::upcast(collisionObject);
|
||||
if (softBody)
|
||||
{
|
||||
btSoftBody::sRayCast softResult;
|
||||
if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
|
||||
{
|
||||
if (softResult.fraction <= resultCallback.m_closestHitFraction)
|
||||
{
|
||||
btCollisionWorld::LocalShapeInfo shapeInfo;
|
||||
shapeInfo.m_shapePart = 0;
|
||||
shapeInfo.m_triangleIndex = softResult.index;
|
||||
// get the normal
|
||||
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
|
||||
btVector3 normal = -rayDir;
|
||||
normal.normalize();
|
||||
{
|
||||
normal = softBody->m_faces[softResult.index].m_normal;
|
||||
if (normal.dot(rayDir) > 0)
|
||||
{
|
||||
// normal always point toward origin of the ray
|
||||
normal = -normal;
|
||||
}
|
||||
}
|
||||
|
||||
btCollisionWorld::LocalRayResult rayResult(collisionObject,
|
||||
&shapeInfo,
|
||||
normal,
|
||||
softResult.fraction);
|
||||
bool normalInWorldSpace = true;
|
||||
resultCallback.addSingleResult(rayResult, normalInWorldSpace);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
@ -23,30 +23,30 @@ subject to the following restrictions:
|
||||
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda; // Lame Parameters
|
||||
btScalar m_E, m_nu; // Young's modulus and Poisson ratio
|
||||
btScalar m_mu_damp, m_lambda_damp;
|
||||
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
|
||||
{
|
||||
btScalar damping = 0.05;
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
btScalar m_mu, m_lambda; // Lame Parameters
|
||||
btScalar m_E, m_nu; // Young's modulus and Poisson ratio
|
||||
btScalar m_mu_damp, m_lambda_damp;
|
||||
btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
|
||||
{
|
||||
btScalar damping = 0.05;
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
|
||||
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
|
||||
{
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
}
|
||||
|
||||
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
|
||||
{
|
||||
m_mu_damp = damping * m_mu;
|
||||
m_lambda_damp = damping * m_lambda;
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
}
|
||||
|
||||
void updateYoungsModulusAndPoissonRatio()
|
||||
{
|
||||
// conversion from Lame Parameters to Young's modulus and Poisson ratio
|
||||
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
|
||||
m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu);
|
||||
m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
|
||||
m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
|
||||
}
|
||||
|
||||
@ -55,21 +55,21 @@ public:
|
||||
// conversion from Young's modulus and Poisson ratio to Lame Parameters
|
||||
// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
|
||||
m_mu = m_E * 0.5 / (1 + m_nu);
|
||||
m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu));
|
||||
m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
|
||||
}
|
||||
|
||||
void setYoungsModulus(btScalar E)
|
||||
{
|
||||
void setYoungsModulus(btScalar E)
|
||||
{
|
||||
m_E = E;
|
||||
updateLameParameters();
|
||||
}
|
||||
}
|
||||
|
||||
void setPoissonRatio(btScalar nu)
|
||||
{
|
||||
m_nu = nu;
|
||||
updateLameParameters();
|
||||
}
|
||||
|
||||
|
||||
void setDamping(btScalar damping)
|
||||
{
|
||||
m_mu_damp = damping * m_mu;
|
||||
@ -83,339 +83,338 @@ public:
|
||||
updateYoungsModulusAndPoissonRatio();
|
||||
}
|
||||
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
virtual void addScaledForces(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledDampingForce(scale, force);
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * df_on_node0;
|
||||
force[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
|
||||
energy += tetra.m_element_measure * elasticEnergyDensity(s);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz+1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
|
||||
{
|
||||
double density = 0;
|
||||
density += m_mu * 0.5 * (s.m_trace - 3.);
|
||||
density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda);
|
||||
density -= m_mu * 0.5 * log(s.m_trace+1);
|
||||
return density;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar max_p = psb->m_cfg.m_maxStress;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j],P);
|
||||
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
addScaledElasticForce(scale, force);
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * df_on_node0;
|
||||
force[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual double totalElasticEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
|
||||
energy += tetra.m_element_measure * elasticEnergyDensity(s);
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual double totalDampingEnergy(btScalar dt)
|
||||
{
|
||||
double energy = 0;
|
||||
int sz = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
sz = btMax(sz, psb->m_nodes[j].index);
|
||||
}
|
||||
}
|
||||
TVStack dampingForce;
|
||||
dampingForce.resize(sz + 1);
|
||||
for (int i = 0; i < dampingForce.size(); ++i)
|
||||
dampingForce[i].setZero();
|
||||
addScaledDampingForce(0.5, dampingForce);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
energy -= dampingForce[node.index].dot(node.m_v) / dt;
|
||||
}
|
||||
}
|
||||
return energy;
|
||||
}
|
||||
|
||||
double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
|
||||
{
|
||||
double density = 0;
|
||||
density += m_mu * 0.5 * (s.m_trace - 3.);
|
||||
density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
|
||||
density -= m_mu * 0.5 * log(s.m_trace + 1);
|
||||
return density;
|
||||
}
|
||||
|
||||
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= force.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
btScalar max_p = psb->m_cfg.m_maxStress;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 P;
|
||||
firstPiola(psb->m_tetraScratches[j], P);
|
||||
#ifdef USE_SVD
|
||||
if (max_p > 0)
|
||||
{
|
||||
// since we want to clamp the principal stress to max_p, we only need to
|
||||
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
|
||||
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
|
||||
if (trPTP > max_p * max_p)
|
||||
{
|
||||
btMatrix3x3 U, V;
|
||||
btVector3 sigma;
|
||||
singularValueDecomposition(P, U, sigma, V);
|
||||
sigma[0] = btMin(sigma[0], max_p);
|
||||
sigma[1] = btMin(sigma[1], max_p);
|
||||
sigma[2] = btMin(sigma[2], max_p);
|
||||
sigma[0] = btMax(sigma[0], -max_p);
|
||||
sigma[1] = btMax(sigma[1], -max_p);
|
||||
sigma[2] = btMax(sigma[2], -max_p);
|
||||
btMatrix3x3 Sigma;
|
||||
Sigma.setIdentity();
|
||||
Sigma[0][0] = sigma[0];
|
||||
Sigma[1][1] = sigma[1];
|
||||
Sigma[2][2] = sigma[2];
|
||||
P = U * Sigma * V.transpose();
|
||||
}
|
||||
}
|
||||
if (max_p > 0)
|
||||
{
|
||||
// since we want to clamp the principal stress to max_p, we only need to
|
||||
// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
|
||||
btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
|
||||
if (trPTP > max_p * max_p)
|
||||
{
|
||||
btMatrix3x3 U, V;
|
||||
btVector3 sigma;
|
||||
singularValueDecomposition(P, U, sigma, V);
|
||||
sigma[0] = btMin(sigma[0], max_p);
|
||||
sigma[1] = btMin(sigma[1], max_p);
|
||||
sigma[2] = btMin(sigma[2], max_p);
|
||||
sigma[0] = btMax(sigma[0], -max_p);
|
||||
sigma[1] = btMax(sigma[1], -max_p);
|
||||
sigma[2] = btMax(sigma[2], -max_p);
|
||||
btMatrix3x3 Sigma;
|
||||
Sigma.setIdentity();
|
||||
Sigma[0][0] = sigma[0];
|
||||
Sigma[1][1] = sigma[1];
|
||||
Sigma[2][2] = sigma[2];
|
||||
P = U * Sigma * V.transpose();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 dP;
|
||||
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// elastic force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
||||
{
|
||||
btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
|
||||
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
|
||||
P = s.m_F * c1 + s.m_cofF * c2;
|
||||
}
|
||||
|
||||
// Let P be the first piola stress.
|
||||
// This function calculates the dP = dP/dF * dF
|
||||
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
|
||||
btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
|
||||
btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
|
||||
dP = dF * c1 + s.m_F * c2;
|
||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
|
||||
dP += s.m_cofF * c3;
|
||||
}
|
||||
|
||||
// Let Q be the damping stress.
|
||||
// This function calculates the dP = dQ/dF * dF
|
||||
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
|
||||
btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
|
||||
btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
|
||||
dP = dF * c1 + s.m_F * c2;
|
||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
|
||||
dP += s.m_cofF * c3;
|
||||
}
|
||||
|
||||
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
|
||||
{
|
||||
btScalar ans = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
ans += A[i].dot(B[i]);
|
||||
}
|
||||
return ans;
|
||||
}
|
||||
|
||||
// Let C(A) be the cofactor of the matrix A
|
||||
// Let H = the derivative of C(A) with respect to A evaluated at F = A
|
||||
// This function calculates H*dF
|
||||
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
|
||||
{
|
||||
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
|
||||
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
|
||||
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
|
||||
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
|
||||
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
|
||||
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
|
||||
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
|
||||
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
|
||||
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_NEOHOOKEAN_FORCE;
|
||||
}
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
|
||||
// elastic force
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
force[id0] -= scale1 * force_on_node0;
|
||||
force[id1] -= scale1 * force_on_node123.getColumn(0);
|
||||
force[id2] -= scale1 * force_on_node123.getColumn(1);
|
||||
force[id3] -= scale1 * force_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
|
||||
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
|
||||
{
|
||||
if (m_mu_damp == 0 && m_lambda_damp == 0)
|
||||
return;
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 I;
|
||||
I.setIdentity();
|
||||
btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
|
||||
// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// damping force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
|
||||
|
||||
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
|
||||
{
|
||||
int numNodes = getNumNodes();
|
||||
btAssert(numNodes <= df.size());
|
||||
btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
if (!psb->isActive())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||
btSoftBody::Node* node2 = tetra.m_n[2];
|
||||
btSoftBody::Node* node3 = tetra.m_n[3];
|
||||
size_t id0 = node0->index;
|
||||
size_t id1 = node1->index;
|
||||
size_t id2 = node2->index;
|
||||
size_t id3 = node3->index;
|
||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
||||
btMatrix3x3 dP;
|
||||
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
||||
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// elastic force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
df[id0] -= scale1 * df_on_node0;
|
||||
df[id1] -= scale1 * df_on_node123.getColumn(0);
|
||||
df[id2] -= scale1 * df_on_node123.getColumn(1);
|
||||
df[id3] -= scale1 * df_on_node123.getColumn(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
||||
{
|
||||
btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
|
||||
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
|
||||
P = s.m_F * c1 + s.m_cofF * c2;
|
||||
}
|
||||
|
||||
// Let P be the first piola stress.
|
||||
// This function calculates the dP = dP/dF * dF
|
||||
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
|
||||
btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
|
||||
btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
|
||||
dP = dF * c1 + s.m_F * c2;
|
||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
|
||||
dP += s.m_cofF * c3;
|
||||
}
|
||||
|
||||
// Let Q be the damping stress.
|
||||
// This function calculates the dP = dQ/dF * dF
|
||||
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||
{
|
||||
btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
|
||||
btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
|
||||
btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
|
||||
dP = dF * c1 + s.m_F * c2;
|
||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
|
||||
dP += s.m_cofF * c3;
|
||||
}
|
||||
|
||||
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
|
||||
{
|
||||
btScalar ans = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
ans += A[i].dot(B[i]);
|
||||
}
|
||||
return ans;
|
||||
}
|
||||
|
||||
// Let C(A) be the cofactor of the matrix A
|
||||
// Let H = the derivative of C(A) with respect to A evaluated at F = A
|
||||
// This function calculates H*dF
|
||||
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
|
||||
{
|
||||
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
|
||||
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
|
||||
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
|
||||
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
|
||||
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
|
||||
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
|
||||
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
|
||||
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
|
||||
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
|
||||
}
|
||||
|
||||
virtual btDeformableLagrangianForceType getForceType()
|
||||
{
|
||||
return BT_NEOHOOKEAN_FORCE;
|
||||
}
|
||||
};
|
||||
#endif /* BT_NEOHOOKEAN_H */
|
||||
|
107
thirdparty/bullet/BulletSoftBody/btKrylovSolver.h
vendored
Normal file
107
thirdparty/bullet/BulletSoftBody/btKrylovSolver.h
vendored
Normal file
@ -0,0 +1,107 @@
|
||||
/*
|
||||
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
|
||||
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2019 Google Inc. http://bulletphysics.org
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BT_KRYLOV_SOLVER_H
|
||||
#define BT_KRYLOV_SOLVER_H
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <LinearMath/btAlignedObjectArray.h>
|
||||
#include <LinearMath/btVector3.h>
|
||||
#include <LinearMath/btScalar.h>
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
template <class MatrixX>
|
||||
class btKrylovSolver
|
||||
{
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
|
||||
public:
|
||||
int m_maxIterations;
|
||||
btScalar m_tolerance;
|
||||
btKrylovSolver(int maxIterations, btScalar tolerance)
|
||||
: m_maxIterations(maxIterations), m_tolerance(tolerance)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~btKrylovSolver() {}
|
||||
|
||||
virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
|
||||
|
||||
virtual void reinitialize(const TVStack& b) = 0;
|
||||
|
||||
virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// c = a-b
|
||||
btAssert(a.size() == b.size());
|
||||
TVStack c;
|
||||
c.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
{
|
||||
c[i] = a[i] - b[i];
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
|
||||
{
|
||||
return dot(a, a);
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
|
||||
{
|
||||
btScalar ret = 0;
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
ret = btMax(ret, btFabs(a[i][d]));
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
|
||||
{
|
||||
btScalar ans(0);
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
ans += a[i].dot(b[i]);
|
||||
return ans;
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
|
||||
{
|
||||
// result += s*a
|
||||
btAssert(a.size() == result.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] += s * a[i];
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
|
||||
{
|
||||
// result = a*s + b
|
||||
TVStack result;
|
||||
result.resize(a.size());
|
||||
for (int i = 0; i < a.size(); ++i)
|
||||
result[i] = s * a[i] + b[i];
|
||||
return result;
|
||||
}
|
||||
|
||||
virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
|
||||
{
|
||||
m_tolerance = tolerance;
|
||||
}
|
||||
};
|
||||
#endif /* BT_KRYLOV_SOLVER_H */
|
473
thirdparty/bullet/BulletSoftBody/btPreconditioner.h
vendored
473
thirdparty/bullet/BulletSoftBody/btPreconditioner.h
vendored
@ -19,269 +19,266 @@
|
||||
class Preconditioner
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
virtual void operator()(const TVStack& x, TVStack& b) = 0;
|
||||
virtual void reinitialize(bool nodeUpdated) = 0;
|
||||
virtual ~Preconditioner(){}
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
virtual void operator()(const TVStack& x, TVStack& b) = 0;
|
||||
virtual void reinitialize(bool nodeUpdated) = 0;
|
||||
virtual ~Preconditioner() {}
|
||||
};
|
||||
|
||||
class DefaultPreconditioner : public Preconditioner
|
||||
{
|
||||
public:
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
for (int i = 0; i < b.size(); ++i)
|
||||
b[i] = x[i];
|
||||
}
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DefaultPreconditioner(){}
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
for (int i = 0; i < b.size(); ++i)
|
||||
b[i] = x[i];
|
||||
}
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DefaultPreconditioner() {}
|
||||
};
|
||||
|
||||
class MassPreconditioner : public Preconditioner
|
||||
{
|
||||
btAlignedObjectArray<btScalar> m_inv_mass;
|
||||
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
public:
|
||||
MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
|
||||
: m_softBodies(softBodies)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
if (nodeUpdated)
|
||||
{
|
||||
m_inv_mass.clear();
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
m_inv_mass.push_back(psb->m_nodes[j].m_im);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
btAssert(m_inv_mass.size() <= x.size());
|
||||
for (int i = 0; i < m_inv_mass.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_mass[i];
|
||||
}
|
||||
for (int i = m_inv_mass.size(); i < b.size(); ++i)
|
||||
{
|
||||
b[i] = x[i];
|
||||
}
|
||||
}
|
||||
};
|
||||
btAlignedObjectArray<btScalar> m_inv_mass;
|
||||
const btAlignedObjectArray<btSoftBody*>& m_softBodies;
|
||||
|
||||
public:
|
||||
MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
|
||||
: m_softBodies(softBodies)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
if (nodeUpdated)
|
||||
{
|
||||
m_inv_mass.clear();
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
m_inv_mass.push_back(psb->m_nodes[j].m_im);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
btAssert(m_inv_mass.size() <= x.size());
|
||||
for (int i = 0; i < m_inv_mass.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_mass[i];
|
||||
}
|
||||
for (int i = m_inv_mass.size(); i < b.size(); ++i)
|
||||
{
|
||||
b[i] = x[i];
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class KKTPreconditioner : public Preconditioner
|
||||
{
|
||||
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
const btDeformableContactProjection& m_projections;
|
||||
const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
|
||||
TVStack m_inv_A, m_inv_S;
|
||||
const btScalar& m_dt;
|
||||
const bool& m_implicit;
|
||||
const btAlignedObjectArray<btSoftBody*>& m_softBodies;
|
||||
const btDeformableContactProjection& m_projections;
|
||||
const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
|
||||
TVStack m_inv_A, m_inv_S;
|
||||
const btScalar& m_dt;
|
||||
const bool& m_implicit;
|
||||
|
||||
public:
|
||||
KKTPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
|
||||
: m_softBodies(softBodies)
|
||||
, m_projections(projections)
|
||||
, m_lf(lf)
|
||||
, m_dt(dt)
|
||||
, m_implicit(implicit)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
if (nodeUpdated)
|
||||
{
|
||||
int num_nodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
num_nodes += psb->m_nodes.size();
|
||||
}
|
||||
m_inv_A.resize(num_nodes);
|
||||
}
|
||||
buildDiagonalA(m_inv_A);
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
// printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0/ m_inv_A[i][d];
|
||||
}
|
||||
}
|
||||
m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
|
||||
// printf("S.size() = %d \n", m_inv_S.size());
|
||||
buildDiagonalS(m_inv_A, m_inv_S);
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
// printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0/ m_inv_S[i][d];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void buildDiagonalA(TVStack& diagA) const
|
||||
{
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
diagA[counter] = (node.m_im == 0) ? btVector3(0,0,0) : btVector3(1.0/node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
if (m_implicit)
|
||||
{
|
||||
printf("implicit not implemented\n");
|
||||
btAssert(false);
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// add damping matrix
|
||||
m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
|
||||
}
|
||||
}
|
||||
|
||||
void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
|
||||
{
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
btVector3& t = diagS[c];
|
||||
t.setZero();
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#define USE_FULL_PRECONDITIONER
|
||||
KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
|
||||
: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void reinitialize(bool nodeUpdated)
|
||||
{
|
||||
if (nodeUpdated)
|
||||
{
|
||||
int num_nodes = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
num_nodes += psb->m_nodes.size();
|
||||
}
|
||||
m_inv_A.resize(num_nodes);
|
||||
}
|
||||
buildDiagonalA(m_inv_A);
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
// printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
|
||||
}
|
||||
}
|
||||
m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
|
||||
// printf("S.size() = %d \n", m_inv_S.size());
|
||||
buildDiagonalS(m_inv_A, m_inv_S);
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
// printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void buildDiagonalA(TVStack& diagA) const
|
||||
{
|
||||
size_t counter = 0;
|
||||
for (int i = 0; i < m_softBodies.size(); ++i)
|
||||
{
|
||||
btSoftBody* psb = m_softBodies[i];
|
||||
for (int j = 0; j < psb->m_nodes.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Node& node = psb->m_nodes[j];
|
||||
diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
|
||||
++counter;
|
||||
}
|
||||
}
|
||||
if (m_implicit)
|
||||
{
|
||||
printf("implicit not implemented\n");
|
||||
btAssert(false);
|
||||
}
|
||||
for (int i = 0; i < m_lf.size(); ++i)
|
||||
{
|
||||
// add damping matrix
|
||||
m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
|
||||
}
|
||||
}
|
||||
|
||||
void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
|
||||
{
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
btVector3& t = diagS[c];
|
||||
t.setZero();
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int d = 0; d < 3; ++d)
|
||||
{
|
||||
t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//#define USE_FULL_PRECONDITIONER
|
||||
#ifndef USE_FULL_PRECONDITIONER
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_A[i];
|
||||
}
|
||||
int offset = m_inv_A.size();
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i+offset] = x[i+offset] * m_inv_S[i];
|
||||
}
|
||||
}
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_A[i];
|
||||
}
|
||||
int offset = m_inv_A.size();
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i + offset] = x[i + offset] * m_inv_S[i];
|
||||
}
|
||||
}
|
||||
#else
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
int offset = m_inv_A.size();
|
||||
virtual void operator()(const TVStack& x, TVStack& b)
|
||||
{
|
||||
btAssert(b.size() == x.size());
|
||||
int offset = m_inv_A.size();
|
||||
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_A[i];
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = x[i] * m_inv_A[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i+offset].setZero();
|
||||
}
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i + offset].setZero();
|
||||
}
|
||||
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
// C * x
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
b[offset+c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
// C * x
|
||||
for (int d = 0; d < lm.m_num_constraints; ++d)
|
||||
{
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i+offset] = b[i+offset] * m_inv_S[i];
|
||||
}
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i + offset] = b[i + offset] * m_inv_S[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i].setZero();
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i].setZero();
|
||||
}
|
||||
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
b[lm.m_indices[i]] += b[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = (x[i] - b[i]) * m_inv_A[i];
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] = (x[i] - b[i]) * m_inv_A[i];
|
||||
}
|
||||
|
||||
TVStack t;
|
||||
t.resize(b.size());
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
t[i+offset] = x[i+offset] * m_inv_S[i];
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
t[i].setZero();
|
||||
}
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
t[lm.m_indices[i]] += t[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] += t[i] * m_inv_A[i];
|
||||
}
|
||||
TVStack t;
|
||||
t.resize(b.size());
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
t[i + offset] = x[i + offset] * m_inv_S[i];
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
t[i].setZero();
|
||||
}
|
||||
for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
|
||||
{
|
||||
// C^T * lambda
|
||||
const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
|
||||
for (int i = 0; i < lm.m_num_nodes; ++i)
|
||||
{
|
||||
for (int j = 0; j < lm.m_num_constraints; ++j)
|
||||
{
|
||||
t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_inv_A.size(); ++i)
|
||||
{
|
||||
b[i] += t[i] * m_inv_A[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i+offset] -= x[i+offset] * m_inv_S[i];
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < m_inv_S.size(); ++i)
|
||||
{
|
||||
b[i + offset] -= x[i + offset] * m_inv_S[i];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
|
1411
thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
vendored
1411
thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
vendored
File diff suppressed because it is too large
Load Diff
418
thirdparty/bullet/BulletSoftBody/btSoftBody.h
vendored
418
thirdparty/bullet/BulletSoftBody/btSoftBody.h
vendored
@ -35,7 +35,7 @@ subject to the following restrictions:
|
||||
//#else
|
||||
#define btSoftBodyData btSoftBodyFloatData
|
||||
#define btSoftBodyDataName "btSoftBodyFloatData"
|
||||
static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
|
||||
static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
|
||||
static unsigned long seed = 243703;
|
||||
//#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
@ -171,10 +171,10 @@ public:
|
||||
CL_SELF = 0x0040, ///Cluster soft body self collision
|
||||
VF_DD = 0x0080, ///Vertex vs face soft vs soft handling
|
||||
|
||||
RVDFmask = 0x0f00, /// Rigid versus deformable face mask
|
||||
SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face
|
||||
SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face
|
||||
SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node
|
||||
RVDFmask = 0x0f00, /// Rigid versus deformable face mask
|
||||
SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face
|
||||
SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face
|
||||
SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node
|
||||
/* presets */
|
||||
Default = SDF_RS,
|
||||
END
|
||||
@ -226,7 +226,7 @@ public:
|
||||
const btCollisionObject* m_colObj; /* Rigid body */
|
||||
btVector3 m_normal; /* Outward normal */
|
||||
btScalar m_offset; /* Offset from origin */
|
||||
btVector3 m_bary; /* Barycentric weights for faces */
|
||||
btVector3 m_bary; /* Barycentric weights for faces */
|
||||
};
|
||||
|
||||
/* sMedium */
|
||||
@ -258,20 +258,29 @@ public:
|
||||
Material* m_material; // Material
|
||||
};
|
||||
/* Node */
|
||||
struct RenderNode
|
||||
{
|
||||
btVector3 m_x;
|
||||
btVector3 m_uv1;
|
||||
btVector3 m_normal;
|
||||
};
|
||||
struct Node : Feature
|
||||
{
|
||||
btVector3 m_x; // Position
|
||||
btVector3 m_q; // Previous step position/Test position
|
||||
btVector3 m_v; // Velocity
|
||||
btVector3 m_vn; // Previous step velocity
|
||||
btVector3 m_vn; // Previous step velocity
|
||||
btVector3 m_f; // Force accumulator
|
||||
btVector3 m_n; // Normal
|
||||
btScalar m_im; // 1/mass
|
||||
btScalar m_area; // Area
|
||||
btDbvtNode* m_leaf; // Leaf data
|
||||
btScalar m_penetration; // depth of penetration
|
||||
int m_constrained; // depth of penetration
|
||||
int m_battach : 1; // Attached
|
||||
int index;
|
||||
int index;
|
||||
btVector3 m_splitv; // velocity associated with split impulse
|
||||
btMatrix3x3 m_effectiveMass; // effective mass in contact
|
||||
btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass
|
||||
};
|
||||
/* Link */
|
||||
ATTRIBUTE_ALIGNED16(struct)
|
||||
@ -287,40 +296,47 @@ public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
};
|
||||
struct RenderFace
|
||||
{
|
||||
RenderNode* m_n[3]; // Node pointers
|
||||
};
|
||||
|
||||
/* Face */
|
||||
struct Face : Feature
|
||||
{
|
||||
Node* m_n[3]; // Node pointers
|
||||
btVector3 m_normal; // Normal
|
||||
btScalar m_ra; // Rest area
|
||||
btDbvtNode* m_leaf; // Leaf data
|
||||
btVector4 m_pcontact; // barycentric weights of the persistent contact
|
||||
btVector3 m_n0, m_n1, m_vn;
|
||||
int m_index;
|
||||
Node* m_n[3]; // Node pointers
|
||||
btVector3 m_normal; // Normal
|
||||
btScalar m_ra; // Rest area
|
||||
btDbvtNode* m_leaf; // Leaf data
|
||||
btVector4 m_pcontact; // barycentric weights of the persistent contact
|
||||
btVector3 m_n0, m_n1, m_vn;
|
||||
int m_index;
|
||||
};
|
||||
/* Tetra */
|
||||
struct Tetra : Feature
|
||||
{
|
||||
Node* m_n[4]; // Node pointers
|
||||
btScalar m_rv; // Rest volume
|
||||
btDbvtNode* m_leaf; // Leaf data
|
||||
btVector3 m_c0[4]; // gradients
|
||||
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
|
||||
btScalar m_c2; // m_c1/sum(|g0..3|^2)
|
||||
btMatrix3x3 m_Dm_inverse; // rest Dm^-1
|
||||
btMatrix3x3 m_F;
|
||||
btScalar m_element_measure;
|
||||
Node* m_n[4]; // Node pointers
|
||||
btScalar m_rv; // Rest volume
|
||||
btDbvtNode* m_leaf; // Leaf data
|
||||
btVector3 m_c0[4]; // gradients
|
||||
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
|
||||
btScalar m_c2; // m_c1/sum(|g0..3|^2)
|
||||
btMatrix3x3 m_Dm_inverse; // rest Dm^-1
|
||||
btMatrix3x3 m_F;
|
||||
btScalar m_element_measure;
|
||||
btVector4 m_P_inv[3]; // first three columns of P_inv matrix
|
||||
};
|
||||
|
||||
/* TetraScratch */
|
||||
struct TetraScratch
|
||||
{
|
||||
btMatrix3x3 m_F; // deformation gradient F
|
||||
btScalar m_trace; // trace of F^T * F
|
||||
btScalar m_J; // det(F)
|
||||
btMatrix3x3 m_cofF; // cofactor of F
|
||||
};
|
||||
|
||||
|
||||
/* TetraScratch */
|
||||
struct TetraScratch
|
||||
{
|
||||
btMatrix3x3 m_F; // deformation gradient F
|
||||
btScalar m_trace; // trace of F^T * F
|
||||
btScalar m_J; // det(F)
|
||||
btMatrix3x3 m_cofF; // cofactor of F
|
||||
btMatrix3x3 m_corotation; // corotatio of the tetra
|
||||
};
|
||||
|
||||
/* RContact */
|
||||
struct RContact
|
||||
{
|
||||
@ -331,67 +347,68 @@ public:
|
||||
btScalar m_c2; // ima*dt
|
||||
btScalar m_c3; // Friction
|
||||
btScalar m_c4; // Hardness
|
||||
|
||||
// jacobians and unit impulse responses for multibody
|
||||
btMultiBodyJacobianData jacobianData_normal;
|
||||
btMultiBodyJacobianData jacobianData_t1;
|
||||
btMultiBodyJacobianData jacobianData_t2;
|
||||
btVector3 t1;
|
||||
btVector3 t2;
|
||||
|
||||
// jacobians and unit impulse responses for multibody
|
||||
btMultiBodyJacobianData jacobianData_normal;
|
||||
btMultiBodyJacobianData jacobianData_t1;
|
||||
btMultiBodyJacobianData jacobianData_t2;
|
||||
btVector3 t1;
|
||||
btVector3 t2;
|
||||
};
|
||||
|
||||
class DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
sCti m_cti; // Contact infos
|
||||
btMatrix3x3 m_c0; // Impulse matrix
|
||||
btVector3 m_c1; // Relative anchor
|
||||
btScalar m_c2; // inverse mass of node/face
|
||||
btScalar m_c3; // Friction
|
||||
btScalar m_c4; // Hardness
|
||||
|
||||
// jacobians and unit impulse responses for multibody
|
||||
btMultiBodyJacobianData jacobianData_normal;
|
||||
btMultiBodyJacobianData jacobianData_t1;
|
||||
btMultiBodyJacobianData jacobianData_t2;
|
||||
btVector3 t1;
|
||||
btVector3 t2;
|
||||
};
|
||||
|
||||
class DeformableNodeRigidContact : public DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
Node* m_node; // Owner node
|
||||
};
|
||||
|
||||
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
|
||||
{
|
||||
public:
|
||||
btVector3 m_local; // Anchor position in body space
|
||||
};
|
||||
|
||||
class DeformableFaceRigidContact : public DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
Face* m_face; // Owner face
|
||||
btVector3 m_contactPoint; // Contact point
|
||||
btVector3 m_bary; // Barycentric weights
|
||||
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
|
||||
};
|
||||
|
||||
struct DeformableFaceNodeContact
|
||||
{
|
||||
Node* m_node; // Node
|
||||
Face* m_face; // Face
|
||||
btVector3 m_bary; // Barycentric weights
|
||||
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
|
||||
btVector3 m_normal; // Normal
|
||||
btScalar m_margin; // Margin
|
||||
btScalar m_friction; // Friction
|
||||
btScalar m_imf; // inverse mass of the face at contact point
|
||||
btScalar m_c0; // scale of the impulse matrix;
|
||||
};
|
||||
|
||||
|
||||
class DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
sCti m_cti; // Contact infos
|
||||
btMatrix3x3 m_c0; // Impulse matrix
|
||||
btVector3 m_c1; // Relative anchor
|
||||
btScalar m_c2; // inverse mass of node/face
|
||||
btScalar m_c3; // Friction
|
||||
btScalar m_c4; // Hardness
|
||||
btMatrix3x3 m_c5; // inverse effective mass
|
||||
|
||||
// jacobians and unit impulse responses for multibody
|
||||
btMultiBodyJacobianData jacobianData_normal;
|
||||
btMultiBodyJacobianData jacobianData_t1;
|
||||
btMultiBodyJacobianData jacobianData_t2;
|
||||
btVector3 t1;
|
||||
btVector3 t2;
|
||||
};
|
||||
|
||||
class DeformableNodeRigidContact : public DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
Node* m_node; // Owner node
|
||||
};
|
||||
|
||||
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
|
||||
{
|
||||
public:
|
||||
btVector3 m_local; // Anchor position in body space
|
||||
};
|
||||
|
||||
class DeformableFaceRigidContact : public DeformableRigidContact
|
||||
{
|
||||
public:
|
||||
Face* m_face; // Owner face
|
||||
btVector3 m_contactPoint; // Contact point
|
||||
btVector3 m_bary; // Barycentric weights
|
||||
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
|
||||
};
|
||||
|
||||
struct DeformableFaceNodeContact
|
||||
{
|
||||
Node* m_node; // Node
|
||||
Face* m_face; // Face
|
||||
btVector3 m_bary; // Barycentric weights
|
||||
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
|
||||
btVector3 m_normal; // Normal
|
||||
btScalar m_margin; // Margin
|
||||
btScalar m_friction; // Friction
|
||||
btScalar m_imf; // inverse mass of the face at contact point
|
||||
btScalar m_c0; // scale of the impulse matrix;
|
||||
};
|
||||
|
||||
/* SContact */
|
||||
struct SContact
|
||||
{
|
||||
@ -718,19 +735,19 @@ public:
|
||||
tVSolverArray m_vsequence; // Velocity solvers sequence
|
||||
tPSolverArray m_psequence; // Position solvers sequence
|
||||
tPSolverArray m_dsequence; // Drift solvers sequence
|
||||
btScalar drag; // deformable air drag
|
||||
btScalar m_maxStress; // Maximum principle first Piola stress
|
||||
btScalar drag; // deformable air drag
|
||||
btScalar m_maxStress; // Maximum principle first Piola stress
|
||||
};
|
||||
/* SolverState */
|
||||
struct SolverState
|
||||
{
|
||||
//if you add new variables, always initialize them!
|
||||
SolverState()
|
||||
:sdt(0),
|
||||
isdt(0),
|
||||
velmrg(0),
|
||||
radmrg(0),
|
||||
updmrg(0)
|
||||
: sdt(0),
|
||||
isdt(0),
|
||||
velmrg(0),
|
||||
radmrg(0),
|
||||
updmrg(0)
|
||||
{
|
||||
}
|
||||
btScalar sdt; // dt*timescale
|
||||
@ -769,9 +786,11 @@ public:
|
||||
typedef btAlignedObjectArray<Cluster*> tClusterArray;
|
||||
typedef btAlignedObjectArray<Note> tNoteArray;
|
||||
typedef btAlignedObjectArray<Node> tNodeArray;
|
||||
typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
|
||||
typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
|
||||
typedef btAlignedObjectArray<Link> tLinkArray;
|
||||
typedef btAlignedObjectArray<Face> tFaceArray;
|
||||
typedef btAlignedObjectArray<RenderFace> tRenderFaceArray;
|
||||
typedef btAlignedObjectArray<Tetra> tTetraArray;
|
||||
typedef btAlignedObjectArray<Anchor> tAnchorArray;
|
||||
typedef btAlignedObjectArray<RContact> tRContactArray;
|
||||
@ -791,40 +810,42 @@ public:
|
||||
btSoftBodyWorldInfo* m_worldInfo; // World info
|
||||
tNoteArray m_notes; // Notes
|
||||
tNodeArray m_nodes; // Nodes
|
||||
tNodeArray m_renderNodes; // Nodes
|
||||
tRenderNodeArray m_renderNodes; // Render Nodes
|
||||
tLinkArray m_links; // Links
|
||||
tFaceArray m_faces; // Faces
|
||||
tFaceArray m_renderFaces; // Faces
|
||||
tRenderFaceArray m_renderFaces; // Faces
|
||||
tTetraArray m_tetras; // Tetras
|
||||
btAlignedObjectArray<TetraScratch> m_tetraScratches;
|
||||
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
|
||||
tAnchorArray m_anchors; // Anchors
|
||||
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
|
||||
tRContactArray m_rcontacts; // Rigid contacts
|
||||
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
|
||||
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
|
||||
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
|
||||
tSContactArray m_scontacts; // Soft contacts
|
||||
tJointArray m_joints; // Joints
|
||||
tMaterialArray m_materials; // Materials
|
||||
btScalar m_timeacc; // Time accumulator
|
||||
btVector3 m_bounds[2]; // Spatial bounds
|
||||
bool m_bUpdateRtCst; // Update runtime constants
|
||||
btDbvt m_ndbvt; // Nodes tree
|
||||
btDbvt m_fdbvt; // Faces tree
|
||||
btDbvntNode* m_fdbvnt; // Faces tree with normals
|
||||
btDbvt m_cdbvt; // Clusters tree
|
||||
tClusterArray m_clusters; // Clusters
|
||||
btScalar m_dampingCoefficient; // Damping Coefficient
|
||||
btAlignedObjectArray<TetraScratch> m_tetraScratches;
|
||||
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
|
||||
tAnchorArray m_anchors; // Anchors
|
||||
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
|
||||
tRContactArray m_rcontacts; // Rigid contacts
|
||||
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
|
||||
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
|
||||
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
|
||||
tSContactArray m_scontacts; // Soft contacts
|
||||
tJointArray m_joints; // Joints
|
||||
tMaterialArray m_materials; // Materials
|
||||
btScalar m_timeacc; // Time accumulator
|
||||
btVector3 m_bounds[2]; // Spatial bounds
|
||||
bool m_bUpdateRtCst; // Update runtime constants
|
||||
btDbvt m_ndbvt; // Nodes tree
|
||||
btDbvt m_fdbvt; // Faces tree
|
||||
btDbvntNode* m_fdbvnt; // Faces tree with normals
|
||||
btDbvt m_cdbvt; // Clusters tree
|
||||
tClusterArray m_clusters; // Clusters
|
||||
btScalar m_dampingCoefficient; // Damping Coefficient
|
||||
btScalar m_sleepingThreshold;
|
||||
btScalar m_maxSpeedSquared;
|
||||
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
|
||||
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
|
||||
btScalar m_repulsionStiffness;
|
||||
btAlignedObjectArray<btVector3> m_X; // initial positions
|
||||
btScalar m_gravityFactor;
|
||||
bool m_cacheBarycenter;
|
||||
btAlignedObjectArray<btVector3> m_X; // initial positions
|
||||
|
||||
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
|
||||
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
|
||||
btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation
|
||||
btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation
|
||||
bool m_useSelfCollision;
|
||||
bool m_softSoftCollision;
|
||||
|
||||
@ -856,11 +877,11 @@ public:
|
||||
{
|
||||
return m_worldInfo;
|
||||
}
|
||||
|
||||
void setDampingCoefficient(btScalar damping_coeff)
|
||||
{
|
||||
m_dampingCoefficient = damping_coeff;
|
||||
}
|
||||
|
||||
void setDampingCoefficient(btScalar damping_coeff)
|
||||
{
|
||||
m_dampingCoefficient = damping_coeff;
|
||||
}
|
||||
|
||||
///@todo: avoid internal softbody shape hack and move collision code to collision library
|
||||
virtual void setCollisionShape(btCollisionShape* collisionShape)
|
||||
@ -921,11 +942,12 @@ public:
|
||||
Material* mat = 0);
|
||||
|
||||
/* Append anchor */
|
||||
void appendDeformableAnchor(int node, btRigidBody* body);
|
||||
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
|
||||
void appendAnchor(int node,
|
||||
void appendDeformableAnchor(int node, btRigidBody* body);
|
||||
void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
|
||||
void appendAnchor(int node,
|
||||
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
|
||||
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
|
||||
void removeAnchor(int node);
|
||||
/* Append linear joint */
|
||||
void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1);
|
||||
void appendLinearJoint(const LJoint::Specs& specs, Body body = Body());
|
||||
@ -976,10 +998,10 @@ public:
|
||||
void setLinearVelocity(const btVector3& linVel);
|
||||
/* Set the angular velocity of the center of mass */
|
||||
void setAngularVelocity(const btVector3& angVel);
|
||||
/* Get best fit rigid transform */
|
||||
btTransform getRigidTransform();
|
||||
/* Transform to given pose */
|
||||
void transformTo(const btTransform& trs);
|
||||
/* Get best fit rigid transform */
|
||||
btTransform getRigidTransform();
|
||||
/* Transform to given pose */
|
||||
void transformTo(const btTransform& trs);
|
||||
/* Transform */
|
||||
void transform(const btTransform& trs);
|
||||
/* Translate */
|
||||
@ -1068,11 +1090,11 @@ public:
|
||||
/* defaultCollisionHandlers */
|
||||
void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
|
||||
void defaultCollisionHandler(btSoftBody* psb);
|
||||
void setSelfCollision(bool useSelfCollision);
|
||||
bool useSelfCollision();
|
||||
void updateDeactivation(btScalar timeStep);
|
||||
void setZeroVelocity();
|
||||
bool wantsSleeping();
|
||||
void setSelfCollision(bool useSelfCollision);
|
||||
bool useSelfCollision();
|
||||
void updateDeactivation(btScalar timeStep);
|
||||
void setZeroVelocity();
|
||||
bool wantsSleeping();
|
||||
|
||||
//
|
||||
// Functionality to deal with new accelerated solvers.
|
||||
@ -1151,8 +1173,8 @@ public:
|
||||
void rebuildNodeTree();
|
||||
btVector3 evaluateCom() const;
|
||||
bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
|
||||
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
|
||||
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
|
||||
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
|
||||
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
|
||||
void updateNormals();
|
||||
void updateBounds();
|
||||
void updatePose();
|
||||
@ -1166,14 +1188,16 @@ public:
|
||||
void solveClusters(btScalar sor);
|
||||
void applyClusters(bool drift);
|
||||
void dampClusters();
|
||||
void setSpringStiffness(btScalar k);
|
||||
void initializeDmInverse();
|
||||
void updateDeformation();
|
||||
void advanceDeformation();
|
||||
void setSpringStiffness(btScalar k);
|
||||
void setGravityFactor(btScalar gravFactor);
|
||||
void setCacheBarycenter(bool cacheBarycenter);
|
||||
void initializeDmInverse();
|
||||
void updateDeformation();
|
||||
void advanceDeformation();
|
||||
void applyForces();
|
||||
void setMaxStress(btScalar maxStress);
|
||||
void interpolateRenderMesh();
|
||||
void setCollisionQuadrature(int N);
|
||||
void setMaxStress(btScalar maxStress);
|
||||
void interpolateRenderMesh();
|
||||
void setCollisionQuadrature(int N);
|
||||
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
|
||||
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
|
||||
static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
|
||||
@ -1182,14 +1206,15 @@ public:
|
||||
static psolver_t getSolver(ePSolver::_ solver);
|
||||
static vsolver_t getSolver(eVSolver::_ solver);
|
||||
void geometricCollisionHandler(btSoftBody* psb);
|
||||
#define SAFE_EPSILON SIMD_EPSILON*100.0
|
||||
#define SAFE_EPSILON SIMD_EPSILON * 100.0
|
||||
void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
|
||||
{
|
||||
if (node->isleaf())
|
||||
{
|
||||
btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
|
||||
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
||||
vol;
|
||||
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
|
||||
if (use_velocity)
|
||||
{
|
||||
btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
|
||||
@ -1207,38 +1232,40 @@ public:
|
||||
{
|
||||
updateNode(node->childs[0], use_velocity, margin);
|
||||
updateNode(node->childs[1], use_velocity, margin);
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
||||
vol;
|
||||
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
|
||||
node->volume = vol;
|
||||
}
|
||||
}
|
||||
|
||||
void updateNodeTree(bool use_velocity, bool margin)
|
||||
|
||||
void updateNodeTree(bool use_velocity, bool margin)
|
||||
{
|
||||
if (m_ndbvt.m_root)
|
||||
updateNode(m_ndbvt.m_root, use_velocity, margin);
|
||||
}
|
||||
|
||||
template <class DBVTNODE> // btDbvtNode or btDbvntNode
|
||||
template <class DBVTNODE> // btDbvtNode or btDbvntNode
|
||||
void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
|
||||
{
|
||||
if (node->isleaf())
|
||||
{
|
||||
btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
|
||||
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
|
||||
btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
||||
vol;
|
||||
if (use_velocity)
|
||||
{
|
||||
btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
|
||||
f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
|
||||
f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
|
||||
f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
|
||||
f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
|
||||
vol = btDbvtVolume::FromPoints(points, 6);
|
||||
}
|
||||
else
|
||||
{
|
||||
btVector3 points[3] = {f->m_n[0]->m_x,
|
||||
f->m_n[1]->m_x,
|
||||
f->m_n[2]->m_x};
|
||||
f->m_n[1]->m_x,
|
||||
f->m_n[2]->m_x};
|
||||
vol = btDbvtVolume::FromPoints(points, 3);
|
||||
}
|
||||
vol.Expand(btVector3(pad, pad, pad));
|
||||
@ -1249,7 +1276,8 @@ public:
|
||||
{
|
||||
updateFace(node->childs[0], use_velocity, margin);
|
||||
updateFace(node->childs[1], use_velocity, margin);
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
||||
vol;
|
||||
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
|
||||
node->volume = vol;
|
||||
}
|
||||
@ -1271,7 +1299,7 @@ public:
|
||||
return (a * coord.x() + b * coord.y() + c * coord.z());
|
||||
}
|
||||
|
||||
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
|
||||
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
|
||||
{
|
||||
btAlignedObjectArray<int> indices;
|
||||
{
|
||||
@ -1297,58 +1325,60 @@ public:
|
||||
const btVector3& n = c.m_normal;
|
||||
btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
|
||||
btScalar d = c.m_margin - n.dot(l);
|
||||
d = btMax(btScalar(0),d);
|
||||
|
||||
d = btMax(btScalar(0), d);
|
||||
|
||||
const btVector3& va = node->m_v;
|
||||
btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
|
||||
btVector3 vr = va - vb;
|
||||
const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing
|
||||
const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing
|
||||
if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
|
||||
continue;
|
||||
btVector3 vt = vr - vn*n;
|
||||
btVector3 vt = vr - vn * n;
|
||||
btScalar I = 0;
|
||||
btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im;
|
||||
btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
|
||||
if (applySpringForce)
|
||||
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
|
||||
if (vn < 0)
|
||||
I += 0.5 * mass * vn;
|
||||
btScalar face_penetration = 0, node_penetration = node->m_penetration;
|
||||
int face_penetration = 0, node_penetration = node->m_constrained;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration);
|
||||
btScalar I_tilde = .5 *I /(1.0+w.length2());
|
||||
|
||||
// double the impulse if node or face is constrained.
|
||||
if (face_penetration > 0 || node_penetration > 0)
|
||||
I_tilde *= 2.0;
|
||||
if (face_penetration <= node_penetration)
|
||||
face_penetration |= face->m_n[i]->m_constrained;
|
||||
btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
|
||||
|
||||
// double the impulse if node or face is constrained.
|
||||
if (face_penetration > 0 || node_penetration > 0)
|
||||
{
|
||||
I_tilde *= 2.0;
|
||||
}
|
||||
if (face_penetration <= 0)
|
||||
{
|
||||
for (int j = 0; j < 3; ++j)
|
||||
face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im;
|
||||
face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im;
|
||||
}
|
||||
if (face_penetration >= node_penetration)
|
||||
if (node_penetration <= 0)
|
||||
{
|
||||
node->m_v -= I_tilde*node->m_im*n;
|
||||
node->m_v -= I_tilde * node->m_im * n;
|
||||
}
|
||||
|
||||
|
||||
// apply frictional impulse
|
||||
btScalar vt_norm = vt.safeNorm();
|
||||
if (vt_norm > SIMD_EPSILON)
|
||||
{
|
||||
btScalar delta_vn = -2 * I * node->m_im;
|
||||
btScalar mu = c.m_friction;
|
||||
btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm;
|
||||
I = 0.5 * mass * (vt_norm-vt_new);
|
||||
btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
|
||||
I = 0.5 * mass * (vt_norm - vt_new);
|
||||
vt.safeNormalize();
|
||||
I_tilde = .5 *I /(1.0+w.length2());
|
||||
// double the impulse if node or face is constrained.
|
||||
// if (face_penetration > 0 || node_penetration > 0)
|
||||
// I_tilde *= 2.0;
|
||||
if (face_penetration <= node_penetration)
|
||||
I_tilde = 2.0 * I / (1.0 + w.length2());
|
||||
// double the impulse if node or face is constrained.
|
||||
if (face_penetration > 0 || node_penetration > 0)
|
||||
I_tilde *= 2.0;
|
||||
if (face_penetration <= 0)
|
||||
{
|
||||
for (int j = 0; j < 3; ++j)
|
||||
face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
|
||||
}
|
||||
if (face_penetration >= node_penetration)
|
||||
if (node_penetration <= 0)
|
||||
{
|
||||
node->m_v -= I_tilde * node->m_im * vt;
|
||||
}
|
||||
@ -1356,7 +1386,7 @@ public:
|
||||
}
|
||||
}
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
|
||||
};
|
||||
|
@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
|
||||
int resy,
|
||||
int fixeds,
|
||||
bool gendiags,
|
||||
btScalar perturbation)
|
||||
btScalar perturbation)
|
||||
{
|
||||
#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
|
||||
/* Create nodes */
|
||||
@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
|
||||
for (int ix = 0; ix < rx; ++ix)
|
||||
{
|
||||
const btScalar tx = ix / (btScalar)(rx - 1);
|
||||
btScalar pert = perturbation * btScalar(rand())/RAND_MAX;
|
||||
btVector3 temp1 = py1;
|
||||
temp1.setY(py1.getY() + pert);
|
||||
btVector3 temp = py0;
|
||||
pert = perturbation * btScalar(rand())/RAND_MAX;
|
||||
temp.setY(py0.getY() + pert);
|
||||
btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
|
||||
btVector3 temp1 = py1;
|
||||
temp1.setY(py1.getY() + pert);
|
||||
btVector3 temp = py0;
|
||||
pert = perturbation * btScalar(rand()) / RAND_MAX;
|
||||
temp.setY(py0.getY() + pert);
|
||||
x[IDX(ix, iy)] = lerp(temp, temp1, tx);
|
||||
m[IDX(ix, iy)] = 1;
|
||||
}
|
||||
@ -1233,9 +1233,9 @@ if(face&&face[0])
|
||||
}
|
||||
}
|
||||
}
|
||||
psb->initializeDmInverse();
|
||||
psb->m_tetraScratches.resize(psb->m_tetras.size());
|
||||
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
|
||||
psb->initializeDmInverse();
|
||||
psb->m_tetraScratches.resize(psb->m_tetras.size());
|
||||
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
|
||||
printf("Nodes: %u\r\n", psb->m_nodes.size());
|
||||
printf("Links: %u\r\n", psb->m_links.size());
|
||||
printf("Faces: %u\r\n", psb->m_faces.size());
|
||||
@ -1245,61 +1245,62 @@ if(face&&face[0])
|
||||
|
||||
btSoftBody* btSoftBodyHelpers::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);
|
||||
X[x_count++] = position;
|
||||
}
|
||||
else if (reading_tets)
|
||||
{
|
||||
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)
|
||||
@ -1308,317 +1309,355 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
|
||||
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;
|
||||
}
|
||||
}
|
||||
btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
|
||||
|
||||
for (int i = 0; i < n_tets; ++i)
|
||||
{
|
||||
const Index& ni = indices[i];
|
||||
psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
|
||||
{
|
||||
psb->appendLink(ni[0], ni[1], 0, true);
|
||||
psb->appendLink(ni[1], ni[2], 0, true);
|
||||
psb->appendLink(ni[2], ni[0], 0, true);
|
||||
psb->appendLink(ni[0], ni[3], 0, true);
|
||||
psb->appendLink(ni[1], ni[3], 0, true);
|
||||
psb->appendLink(ni[2], ni[3], 0, true);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
generateBoundaryFaces(psb);
|
||||
psb->initializeDmInverse();
|
||||
psb->m_tetraScratches.resize(psb->m_tetras.size());
|
||||
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
|
||||
printf("Nodes: %u\r\n", psb->m_nodes.size());
|
||||
printf("Links: %u\r\n", psb->m_links.size());
|
||||
printf("Faces: %u\r\n", psb->m_faces.size());
|
||||
printf("Tetras: %u\r\n", psb->m_tetras.size());
|
||||
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;
|
||||
}
|
||||
}
|
||||
btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
|
||||
|
||||
fs.close();
|
||||
return psb;
|
||||
for (int i = 0; i < n_tets; ++i)
|
||||
{
|
||||
const Index& ni = indices[i];
|
||||
psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
|
||||
{
|
||||
psb->appendLink(ni[0], ni[1], 0, true);
|
||||
psb->appendLink(ni[1], ni[2], 0, true);
|
||||
psb->appendLink(ni[2], ni[0], 0, true);
|
||||
psb->appendLink(ni[0], ni[3], 0, true);
|
||||
psb->appendLink(ni[1], ni[3], 0, true);
|
||||
psb->appendLink(ni[2], ni[3], 0, true);
|
||||
}
|
||||
}
|
||||
|
||||
generateBoundaryFaces(psb);
|
||||
psb->initializeDmInverse();
|
||||
psb->m_tetraScratches.resize(psb->m_tetras.size());
|
||||
psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
|
||||
printf("Nodes: %u\r\n", psb->m_nodes.size());
|
||||
printf("Links: %u\r\n", psb->m_links.size());
|
||||
printf("Faces: %u\r\n", psb->m_faces.size());
|
||||
printf("Tetras: %u\r\n", psb->m_tetras.size());
|
||||
|
||||
fs.close();
|
||||
return psb;
|
||||
}
|
||||
|
||||
void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
|
||||
{
|
||||
int counter = 0;
|
||||
for (int i = 0; i < psb->m_nodes.size(); ++i)
|
||||
{
|
||||
psb->m_nodes[i].index = counter++;
|
||||
}
|
||||
typedef btAlignedObjectArray<int> Index;
|
||||
btAlignedObjectArray<Index> indices;
|
||||
indices.resize(psb->m_tetras.size());
|
||||
for (int i = 0; i < indices.size(); ++i)
|
||||
{
|
||||
Index index;
|
||||
index.push_back(psb->m_tetras[i].m_n[0]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[1]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[2]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[3]->index);
|
||||
indices[i] = index;
|
||||
}
|
||||
|
||||
std::map<std::vector<int>, std::vector<int> > dict;
|
||||
for (int i = 0; i < indices.size(); ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
std::vector<int> f;
|
||||
if (j == 0)
|
||||
{
|
||||
f.push_back(indices[i][1]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][2]);
|
||||
}
|
||||
if (j == 1)
|
||||
{
|
||||
f.push_back(indices[i][3]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][1]);
|
||||
}
|
||||
if (j == 2)
|
||||
{
|
||||
f.push_back(indices[i][3]);
|
||||
f.push_back(indices[i][1]);
|
||||
f.push_back(indices[i][2]);
|
||||
}
|
||||
if (j == 3)
|
||||
{
|
||||
f.push_back(indices[i][2]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][3]);
|
||||
}
|
||||
std::vector<int> f_sorted = f;
|
||||
std::sort(f_sorted.begin(), f_sorted.end());
|
||||
if (dict.find(f_sorted) != dict.end())
|
||||
{
|
||||
dict.erase(f_sorted);
|
||||
}
|
||||
else
|
||||
{
|
||||
dict.insert(std::make_pair(f_sorted, f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
|
||||
{
|
||||
std::vector<int> f = it->second;
|
||||
psb->appendFace(f[0], f[1], f[2]);
|
||||
}
|
||||
int counter = 0;
|
||||
for (int i = 0; i < psb->m_nodes.size(); ++i)
|
||||
{
|
||||
psb->m_nodes[i].index = counter++;
|
||||
}
|
||||
typedef btAlignedObjectArray<int> Index;
|
||||
btAlignedObjectArray<Index> indices;
|
||||
indices.resize(psb->m_tetras.size());
|
||||
for (int i = 0; i < indices.size(); ++i)
|
||||
{
|
||||
Index index;
|
||||
index.push_back(psb->m_tetras[i].m_n[0]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[1]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[2]->index);
|
||||
index.push_back(psb->m_tetras[i].m_n[3]->index);
|
||||
indices[i] = index;
|
||||
}
|
||||
|
||||
std::map<std::vector<int>, std::vector<int> > dict;
|
||||
for (int i = 0; i < indices.size(); ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
std::vector<int> f;
|
||||
if (j == 0)
|
||||
{
|
||||
f.push_back(indices[i][1]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][2]);
|
||||
}
|
||||
if (j == 1)
|
||||
{
|
||||
f.push_back(indices[i][3]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][1]);
|
||||
}
|
||||
if (j == 2)
|
||||
{
|
||||
f.push_back(indices[i][3]);
|
||||
f.push_back(indices[i][1]);
|
||||
f.push_back(indices[i][2]);
|
||||
}
|
||||
if (j == 3)
|
||||
{
|
||||
f.push_back(indices[i][2]);
|
||||
f.push_back(indices[i][0]);
|
||||
f.push_back(indices[i][3]);
|
||||
}
|
||||
std::vector<int> f_sorted = f;
|
||||
std::sort(f_sorted.begin(), f_sorted.end());
|
||||
if (dict.find(f_sorted) != dict.end())
|
||||
{
|
||||
dict.erase(f_sorted);
|
||||
}
|
||||
else
|
||||
{
|
||||
dict.insert(std::make_pair(f_sorted, f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
|
||||
{
|
||||
std::vector<int> f = it->second;
|
||||
psb->appendFace(f[0], f[1], f[2]);
|
||||
//printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1);
|
||||
}
|
||||
}
|
||||
|
||||
//Write the surface mesh to an obj file.
|
||||
void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
|
||||
{
|
||||
std::ofstream fs;
|
||||
fs.open(filename);
|
||||
btAssert(fs);
|
||||
for (int i = 0; i < psb->m_nodes.size(); ++i)
|
||||
{
|
||||
fs << "v";
|
||||
for (int d = 0; d < 3; d++)
|
||||
{
|
||||
fs << " " << psb->m_nodes[i].m_x[d];
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
|
||||
for (int i = 0; i < psb->m_faces.size(); ++i)
|
||||
{
|
||||
fs << "f";
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
fs << " " << psb->m_faces[i].m_n[n]->index + 1;
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
fs.close();
|
||||
std::ofstream fs;
|
||||
fs.open(filename);
|
||||
btAssert(fs);
|
||||
|
||||
if (psb->m_tetras.size() > 0)
|
||||
{
|
||||
// For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
|
||||
std::map<int, int> dict;
|
||||
for (int i = 0; i < psb->m_faces.size(); i++)
|
||||
{
|
||||
for (int d = 0; d < 3; d++)
|
||||
{
|
||||
int index = psb->m_faces[i].m_n[d]->index;
|
||||
if (dict.find(index) == dict.end())
|
||||
{
|
||||
int dict_size = dict.size();
|
||||
dict[index] = dict_size;
|
||||
fs << "v";
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
fs << " " << psb->m_nodes[index].m_x[k];
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
// Write surface mesh.
|
||||
for (int i = 0; i < psb->m_faces.size(); ++i)
|
||||
{
|
||||
fs << "f";
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// For trimesh, directly write out all the nodes and faces.xs
|
||||
for (int i = 0; i < psb->m_nodes.size(); ++i)
|
||||
{
|
||||
fs << "v";
|
||||
for (int d = 0; d < 3; d++)
|
||||
{
|
||||
fs << " " << psb->m_nodes[i].m_x[d];
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
|
||||
for (int i = 0; i < psb->m_faces.size(); ++i)
|
||||
{
|
||||
fs << "f";
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
fs << " " << psb->m_faces[i].m_n[n]->index + 1;
|
||||
}
|
||||
fs << "\n";
|
||||
}
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
|
||||
{
|
||||
std::ifstream fs_read;
|
||||
fs_read.open(filename);
|
||||
std::string line;
|
||||
btVector3 pos;
|
||||
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
|
||||
while (std::getline(fs_read, line))
|
||||
{
|
||||
std::stringstream ss(line);
|
||||
if (line[0] == 'v')
|
||||
{
|
||||
}
|
||||
else if (line[0] == 'f')
|
||||
{
|
||||
ss.ignore();
|
||||
int id0, id1, id2;
|
||||
ss >> id0;
|
||||
ss >> id1;
|
||||
ss >> id2;
|
||||
btAlignedObjectArray<int> new_face;
|
||||
new_face.push_back(id1);
|
||||
new_face.push_back(id0);
|
||||
new_face.push_back(id2);
|
||||
additional_faces.push_back(new_face);
|
||||
}
|
||||
}
|
||||
fs_read.close();
|
||||
std::ifstream fs_read;
|
||||
fs_read.open(filename);
|
||||
std::string line;
|
||||
btVector3 pos;
|
||||
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
|
||||
while (std::getline(fs_read, line))
|
||||
{
|
||||
std::stringstream ss(line);
|
||||
if (line[0] == 'v')
|
||||
{
|
||||
}
|
||||
else if (line[0] == 'f')
|
||||
{
|
||||
ss.ignore();
|
||||
int id0, id1, id2;
|
||||
ss >> id0;
|
||||
ss >> id1;
|
||||
ss >> id2;
|
||||
btAlignedObjectArray<int> new_face;
|
||||
new_face.push_back(id1);
|
||||
new_face.push_back(id0);
|
||||
new_face.push_back(id2);
|
||||
additional_faces.push_back(new_face);
|
||||
}
|
||||
}
|
||||
fs_read.close();
|
||||
|
||||
std::ofstream fs_write;
|
||||
fs_write.open(filename, std::ios_base::app);
|
||||
for (int i = 0; i < additional_faces.size(); ++i)
|
||||
{
|
||||
fs_write << "f";
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
fs_write << " " << additional_faces[i][n];
|
||||
}
|
||||
fs_write << "\n";
|
||||
}
|
||||
fs_write.close();
|
||||
std::ofstream fs_write;
|
||||
fs_write.open(filename, std::ios_base::app);
|
||||
for (int i = 0; i < additional_faces.size(); ++i)
|
||||
{
|
||||
fs_write << "f";
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
fs_write << " " << additional_faces[i][n];
|
||||
}
|
||||
fs_write << "\n";
|
||||
}
|
||||
fs_write.close();
|
||||
}
|
||||
|
||||
// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
|
||||
void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
|
||||
{
|
||||
btVector3 vap = p - a;
|
||||
btVector3 vbp = p - b;
|
||||
|
||||
btVector3 vab = b - a;
|
||||
btVector3 vac = c - a;
|
||||
btVector3 vad = d - a;
|
||||
|
||||
btVector3 vbc = c - b;
|
||||
btVector3 vbd = d - b;
|
||||
btScalar va6 = (vbp.cross(vbd)).dot(vbc);
|
||||
btScalar vb6 = (vap.cross(vac)).dot(vad);
|
||||
btScalar vc6 = (vap.cross(vad)).dot(vab);
|
||||
btScalar vd6 = (vap.cross(vab)).dot(vac);
|
||||
btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
|
||||
bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
|
||||
btVector3 vap = p - a;
|
||||
btVector3 vbp = p - b;
|
||||
|
||||
btVector3 vab = b - a;
|
||||
btVector3 vac = c - a;
|
||||
btVector3 vad = d - a;
|
||||
|
||||
btVector3 vbc = c - b;
|
||||
btVector3 vbd = d - b;
|
||||
btScalar va6 = (vbp.cross(vbd)).dot(vbc);
|
||||
btScalar vb6 = (vap.cross(vac)).dot(vad);
|
||||
btScalar vc6 = (vap.cross(vad)).dot(vab);
|
||||
btScalar vd6 = (vap.cross(vab)).dot(vac);
|
||||
btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
|
||||
bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
|
||||
}
|
||||
|
||||
// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
|
||||
void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
|
||||
{
|
||||
btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
|
||||
btScalar d00 = btDot(v0, v0);
|
||||
btScalar d01 = btDot(v0, v1);
|
||||
btScalar d11 = btDot(v1, v1);
|
||||
btScalar d20 = btDot(v2, v0);
|
||||
btScalar d21 = btDot(v2, v1);
|
||||
btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
|
||||
bary[1] = (d11 * d20 - d01 * d21) * invDenom;
|
||||
bary[2] = (d00 * d21 - d01 * d20) * invDenom;
|
||||
bary[0] = 1.0 - bary[1] - bary[2];
|
||||
bary[3] = 0;
|
||||
btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
|
||||
btScalar d00 = btDot(v0, v0);
|
||||
btScalar d01 = btDot(v0, v1);
|
||||
btScalar d11 = btDot(v1, v1);
|
||||
btScalar d20 = btDot(v2, v0);
|
||||
btScalar d21 = btDot(v2, v1);
|
||||
btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
|
||||
bary[1] = (d11 * d20 - d01 * d21) * invDenom;
|
||||
bary[2] = (d00 * d21 - d01 * d20) * invDenom;
|
||||
bary[0] = 1.0 - bary[1] - bary[2];
|
||||
bary[3] = 0;
|
||||
}
|
||||
|
||||
// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
|
||||
// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
|
||||
void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
|
||||
{
|
||||
psb->m_z.resize(0);
|
||||
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
|
||||
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
|
||||
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
|
||||
{
|
||||
const btVector3& p = psb->m_renderNodes[i].m_x;
|
||||
btVector4 bary;
|
||||
btVector4 optimal_bary;
|
||||
btScalar min_bary_weight = -1e3;
|
||||
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Tetra& t = psb->m_tetras[j];
|
||||
getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
|
||||
btScalar new_min_bary_weight = bary[0];
|
||||
for (int k = 1; k < 4; ++k)
|
||||
{
|
||||
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
|
||||
}
|
||||
if (new_min_bary_weight > min_bary_weight)
|
||||
{
|
||||
btAlignedObjectArray<const btSoftBody::Node*> parents;
|
||||
parents.push_back(t.m_n[0]);
|
||||
parents.push_back(t.m_n[1]);
|
||||
parents.push_back(t.m_n[2]);
|
||||
parents.push_back(t.m_n[3]);
|
||||
optimal_parents = parents;
|
||||
optimal_bary = bary;
|
||||
min_bary_weight = new_min_bary_weight;
|
||||
// stop searching if p is inside the tetrahedron at hand
|
||||
if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
|
||||
psb->m_renderNodesParents[i] = optimal_parents;
|
||||
}
|
||||
psb->m_z.resize(0);
|
||||
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
|
||||
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
|
||||
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
|
||||
{
|
||||
const btVector3& p = psb->m_renderNodes[i].m_x;
|
||||
btVector4 bary;
|
||||
btVector4 optimal_bary;
|
||||
btScalar min_bary_weight = -1e3;
|
||||
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
|
||||
for (int j = 0; j < psb->m_tetras.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Tetra& t = psb->m_tetras[j];
|
||||
getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
|
||||
btScalar new_min_bary_weight = bary[0];
|
||||
for (int k = 1; k < 4; ++k)
|
||||
{
|
||||
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
|
||||
}
|
||||
if (new_min_bary_weight > min_bary_weight)
|
||||
{
|
||||
btAlignedObjectArray<const btSoftBody::Node*> parents;
|
||||
parents.push_back(t.m_n[0]);
|
||||
parents.push_back(t.m_n[1]);
|
||||
parents.push_back(t.m_n[2]);
|
||||
parents.push_back(t.m_n[3]);
|
||||
optimal_parents = parents;
|
||||
optimal_bary = bary;
|
||||
min_bary_weight = new_min_bary_weight;
|
||||
// stop searching if p is inside the tetrahedron at hand
|
||||
if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
|
||||
psb->m_renderNodesParents[i] = optimal_parents;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
|
||||
void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
|
||||
{
|
||||
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
|
||||
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
|
||||
psb->m_z.resize(psb->m_renderNodes.size());
|
||||
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
|
||||
{
|
||||
const btVector3& p = psb->m_renderNodes[i].m_x;
|
||||
btVector4 bary;
|
||||
btVector4 optimal_bary;
|
||||
btScalar min_bary_weight = -SIMD_INFINITY;
|
||||
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
|
||||
btScalar dist = 0, optimal_dist = 0;
|
||||
for (int j = 0; j < psb->m_faces.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face& f = psb->m_faces[j];
|
||||
btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
|
||||
btVector3 unit_n = n.normalized();
|
||||
dist = (p-f.m_n[0]->m_x).dot(unit_n);
|
||||
btVector3 proj_p = p - dist*unit_n;
|
||||
getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
|
||||
btScalar new_min_bary_weight = bary[0];
|
||||
for (int k = 1; k < 3; ++k)
|
||||
{
|
||||
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
|
||||
psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
|
||||
psb->m_z.resize(psb->m_renderNodes.size());
|
||||
for (int i = 0; i < psb->m_renderNodes.size(); ++i)
|
||||
{
|
||||
const btVector3& p = psb->m_renderNodes[i].m_x;
|
||||
btVector4 bary;
|
||||
btVector4 optimal_bary;
|
||||
btScalar min_bary_weight = -SIMD_INFINITY;
|
||||
btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
|
||||
btScalar dist = 0, optimal_dist = 0;
|
||||
for (int j = 0; j < psb->m_faces.size(); ++j)
|
||||
{
|
||||
const btSoftBody::Face& f = psb->m_faces[j];
|
||||
btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
|
||||
btVector3 unit_n = n.normalized();
|
||||
dist = (p - f.m_n[0]->m_x).dot(unit_n);
|
||||
btVector3 proj_p = p - dist * unit_n;
|
||||
getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
|
||||
btScalar new_min_bary_weight = bary[0];
|
||||
for (int k = 1; k < 3; ++k)
|
||||
{
|
||||
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
|
||||
}
|
||||
|
||||
// p is out of the current best triangle, we found a traingle that's better
|
||||
bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.);
|
||||
// p is inside of the current best triangle, we found a triangle that's better
|
||||
bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
|
||||
// p is out of the current best triangle, we found a traingle that's better
|
||||
bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
|
||||
// p is inside of the current best triangle, we found a triangle that's better
|
||||
bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
|
||||
|
||||
if (better_than_closest_outisde || better_than_best_inside)
|
||||
{
|
||||
btAlignedObjectArray<const btSoftBody::Node*> parents;
|
||||
parents.push_back(f.m_n[0]);
|
||||
parents.push_back(f.m_n[1]);
|
||||
parents.push_back(f.m_n[2]);
|
||||
optimal_parents = parents;
|
||||
optimal_bary = bary;
|
||||
optimal_dist = dist;
|
||||
min_bary_weight = new_min_bary_weight;
|
||||
}
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
|
||||
psb->m_renderNodesParents[i] = optimal_parents;
|
||||
psb->m_z[i] = optimal_dist;
|
||||
}
|
||||
if (better_than_closest_outisde || better_than_best_inside)
|
||||
{
|
||||
btAlignedObjectArray<const btSoftBody::Node*> parents;
|
||||
parents.push_back(f.m_n[0]);
|
||||
parents.push_back(f.m_n[1]);
|
||||
parents.push_back(f.m_n[2]);
|
||||
optimal_parents = parents;
|
||||
optimal_bary = bary;
|
||||
optimal_dist = dist;
|
||||
min_bary_weight = new_min_bary_weight;
|
||||
}
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
|
||||
psb->m_renderNodesParents[i] = optimal_parents;
|
||||
psb->m_z[i] = optimal_dist;
|
||||
}
|
||||
}
|
||||
|
@ -93,7 +93,7 @@ struct btSoftBodyHelpers
|
||||
int resy,
|
||||
int fixeds,
|
||||
bool gendiags,
|
||||
btScalar perturbation = 0.);
|
||||
btScalar perturbation = 0.);
|
||||
/* Create a patch with UV Texture Coordinates */
|
||||
static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
|
||||
const btVector3& corner00,
|
||||
@ -142,21 +142,21 @@ struct btSoftBodyHelpers
|
||||
bool bfacelinks,
|
||||
bool btetralinks,
|
||||
bool bfacesfromtetras);
|
||||
static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
|
||||
static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
|
||||
|
||||
static void writeObj(const char* file, const btSoftBody* psb);
|
||||
|
||||
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
|
||||
|
||||
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
|
||||
|
||||
static void interpolateBarycentricWeights(btSoftBody* psb);
|
||||
|
||||
static void extrapolateBarycentricWeights(btSoftBody* psb);
|
||||
|
||||
static void generateBoundaryFaces(btSoftBody* psb);
|
||||
|
||||
static void duplicateFaces(const char* filename, const btSoftBody* psb);
|
||||
static void writeObj(const char* file, const btSoftBody* psb);
|
||||
|
||||
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
|
||||
|
||||
static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
|
||||
|
||||
static void interpolateBarycentricWeights(btSoftBody* psb);
|
||||
|
||||
static void extrapolateBarycentricWeights(btSoftBody* psb);
|
||||
|
||||
static void generateBoundaryFaces(btSoftBody* psb);
|
||||
|
||||
static void duplicateFaces(const char* filename, const btSoftBody* psb);
|
||||
/// Sort the list of links to move link calculations that are dependent upon earlier
|
||||
/// ones as far as possible away from the calculation of those values
|
||||
/// This tends to make adjacent loop iterations not dependent upon one another,
|
||||
|
2042
thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
vendored
2042
thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
vendored
File diff suppressed because it is too large
Load Diff
@ -36,7 +36,7 @@ public:
|
||||
CL_SIMD_SOLVER,
|
||||
DX_SOLVER,
|
||||
DX_SIMD_SOLVER,
|
||||
DEFORMABLE_SOLVER
|
||||
DEFORMABLE_SOLVER
|
||||
};
|
||||
|
||||
protected:
|
||||
|
@ -100,6 +100,11 @@ void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeSte
|
||||
///update soft bodies
|
||||
m_softBodySolver->updateSoftBodies();
|
||||
|
||||
for (int i = 0; i < m_softBodies.size(); i++)
|
||||
{
|
||||
btSoftBody* psb = (btSoftBody*)m_softBodies[i];
|
||||
psb->interpolateRenderMesh();
|
||||
}
|
||||
// End solver-wise simulation step
|
||||
// ///////////////////////////////
|
||||
}
|
||||
|
68
thirdparty/bullet/BulletSoftBody/btSparseSDF.h
vendored
68
thirdparty/bullet/BulletSoftBody/btSparseSDF.h
vendored
@ -22,36 +22,36 @@ subject to the following restrictions:
|
||||
|
||||
// Fast Hash
|
||||
|
||||
#if !defined (get16bits)
|
||||
#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\
|
||||
+(unsigned int)(((const unsigned char *)(d))[0]) )
|
||||
#if !defined(get16bits)
|
||||
#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
|
||||
#endif
|
||||
//
|
||||
// super hash function by Paul Hsieh
|
||||
//
|
||||
inline unsigned int HsiehHash (const char * data, int len) {
|
||||
unsigned int hash = len, tmp;
|
||||
len>>=2;
|
||||
inline unsigned int HsiehHash(const char* data, int len)
|
||||
{
|
||||
unsigned int hash = len, tmp;
|
||||
len >>= 2;
|
||||
|
||||
/* Main loop */
|
||||
for (;len > 0; len--) {
|
||||
hash += get16bits (data);
|
||||
tmp = (get16bits (data+2) << 11) ^ hash;
|
||||
hash = (hash << 16) ^ tmp;
|
||||
data += 2*sizeof (unsigned short);
|
||||
hash += hash >> 11;
|
||||
}
|
||||
/* Main loop */
|
||||
for (; len > 0; len--)
|
||||
{
|
||||
hash += get16bits(data);
|
||||
tmp = (get16bits(data + 2) << 11) ^ hash;
|
||||
hash = (hash << 16) ^ tmp;
|
||||
data += 2 * sizeof(unsigned short);
|
||||
hash += hash >> 11;
|
||||
}
|
||||
|
||||
/* Force "avalanching" of final 127 bits */
|
||||
hash ^= hash << 3;
|
||||
hash += hash >> 5;
|
||||
hash ^= hash << 4;
|
||||
hash += hash >> 17;
|
||||
hash ^= hash << 25;
|
||||
hash += hash >> 6;
|
||||
|
||||
/* Force "avalanching" of final 127 bits */
|
||||
hash ^= hash << 3;
|
||||
hash += hash >> 5;
|
||||
hash ^= hash << 4;
|
||||
hash += hash >> 17;
|
||||
hash ^= hash << 25;
|
||||
hash += hash >> 6;
|
||||
|
||||
return hash;
|
||||
return hash;
|
||||
}
|
||||
|
||||
template <const int CELLSIZE>
|
||||
@ -81,7 +81,7 @@ struct btSparseSdf
|
||||
|
||||
btAlignedObjectArray<Cell*> cells;
|
||||
btScalar voxelsz;
|
||||
btScalar m_defaultVoxelsz;
|
||||
btScalar m_defaultVoxelsz;
|
||||
int puid;
|
||||
int ncells;
|
||||
int m_clampCells;
|
||||
@ -103,16 +103,16 @@ struct btSparseSdf
|
||||
//if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
|
||||
m_clampCells = clampCells;
|
||||
cells.resize(hashsize, 0);
|
||||
m_defaultVoxelsz = 0.25;
|
||||
m_defaultVoxelsz = 0.25;
|
||||
Reset();
|
||||
}
|
||||
//
|
||||
|
||||
void setDefaultVoxelsz(btScalar sz)
|
||||
{
|
||||
m_defaultVoxelsz = sz;
|
||||
}
|
||||
|
||||
|
||||
void setDefaultVoxelsz(btScalar sz)
|
||||
{
|
||||
m_defaultVoxelsz = sz;
|
||||
}
|
||||
|
||||
void Reset()
|
||||
{
|
||||
for (int i = 0, ni = cells.size(); i < ni; ++i)
|
||||
@ -162,7 +162,7 @@ struct btSparseSdf
|
||||
nqueries = 1;
|
||||
nprobes = 1;
|
||||
++puid; ///@todo: Reset puid's when int range limit is reached */
|
||||
/* else setup a priority list... */
|
||||
/* else setup a priority list... */
|
||||
}
|
||||
//
|
||||
int RemoveReferences(btCollisionShape* pcs)
|
||||
@ -221,7 +221,7 @@ struct btSparseSdf
|
||||
else
|
||||
{
|
||||
// printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
|
||||
//printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
|
||||
//printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
|
||||
|
||||
c = c->next;
|
||||
}
|
||||
@ -363,7 +363,7 @@ struct btSparseSdf
|
||||
myset.p = (void*)shape;
|
||||
const char* ptr = (const char*)&myset;
|
||||
|
||||
unsigned int result = HsiehHash(ptr, sizeof(btS) );
|
||||
unsigned int result = HsiehHash(ptr, sizeof(btS));
|
||||
|
||||
return result;
|
||||
}
|
||||
|
742
thirdparty/bullet/BulletSoftBody/poly34.cpp
vendored
742
thirdparty/bullet/BulletSoftBody/poly34.cpp
vendored
@ -6,7 +6,7 @@
|
||||
//
|
||||
#include <math.h>
|
||||
|
||||
#include "poly34.h" // solution of cubic and quartic equation
|
||||
#include "poly34.h" // solution of cubic and quartic equation
|
||||
#define TwoPi 6.28318530717958648
|
||||
const btScalar eps = SIMD_EPSILON;
|
||||
|
||||
@ -15,50 +15,53 @@ const btScalar eps = SIMD_EPSILON;
|
||||
//=============================================================================
|
||||
static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
|
||||
{
|
||||
btScalar s = 1.;
|
||||
while (x < 1.) {
|
||||
x *= 8.;
|
||||
s *= 0.5;
|
||||
}
|
||||
while (x > 8.) {
|
||||
x *= 0.125;
|
||||
s *= 2.;
|
||||
}
|
||||
btScalar r = 1.5;
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
return r * s;
|
||||
btScalar s = 1.;
|
||||
while (x < 1.)
|
||||
{
|
||||
x *= 8.;
|
||||
s *= 0.5;
|
||||
}
|
||||
while (x > 8.)
|
||||
{
|
||||
x *= 0.125;
|
||||
s *= 2.;
|
||||
}
|
||||
btScalar r = 1.5;
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
r -= 1. / 3. * (r - x / (r * r));
|
||||
return r * s;
|
||||
}
|
||||
|
||||
btScalar SIMD_FORCE_INLINE root3(btScalar x)
|
||||
{
|
||||
if (x > 0)
|
||||
return _root3(x);
|
||||
else if (x < 0)
|
||||
return -_root3(-x);
|
||||
else
|
||||
return 0.;
|
||||
if (x > 0)
|
||||
return _root3(x);
|
||||
else if (x < 0)
|
||||
return -_root3(-x);
|
||||
else
|
||||
return 0.;
|
||||
}
|
||||
|
||||
// x - array of size 2
|
||||
// return 2: 2 real roots x[0], x[1]
|
||||
// return 0: pair of complex roots: x[0]i*x[1]
|
||||
int SolveP2(btScalar* x, btScalar a, btScalar b)
|
||||
{ // solve equation x^2 + a*x + b = 0
|
||||
btScalar D = 0.25 * a * a - b;
|
||||
if (D >= 0) {
|
||||
D = sqrt(D);
|
||||
x[0] = -0.5 * a + D;
|
||||
x[1] = -0.5 * a - D;
|
||||
return 2;
|
||||
}
|
||||
x[0] = -0.5 * a;
|
||||
x[1] = sqrt(-D);
|
||||
return 0;
|
||||
{ // solve equation x^2 + a*x + b = 0
|
||||
btScalar D = 0.25 * a * a - b;
|
||||
if (D >= 0)
|
||||
{
|
||||
D = sqrt(D);
|
||||
x[0] = -0.5 * a + D;
|
||||
x[1] = -0.5 * a - D;
|
||||
return 2;
|
||||
}
|
||||
x[0] = -0.5 * a;
|
||||
x[1] = sqrt(-D);
|
||||
return 0;
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
// x - array of size 3
|
||||
@ -66,217 +69,228 @@ int SolveP2(btScalar* x, btScalar a, btScalar b)
|
||||
// 2 real roots: x[0], x[1], return 2
|
||||
// 1 real root : x[0], x[1] i*x[2], return 1
|
||||
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
|
||||
{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0
|
||||
btScalar a2 = a * a;
|
||||
btScalar q = (a2 - 3 * b) / 9;
|
||||
if (q < 0)
|
||||
q = eps;
|
||||
btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
|
||||
// equation x^3 + q*x + r = 0
|
||||
btScalar r2 = r * r;
|
||||
btScalar q3 = q * q * q;
|
||||
btScalar A, B;
|
||||
if (r2 <= (q3 + eps)) { //<<-- FIXED!
|
||||
btScalar t = r / sqrt(q3);
|
||||
if (t < -1)
|
||||
t = -1;
|
||||
if (t > 1)
|
||||
t = 1;
|
||||
t = acos(t);
|
||||
a /= 3;
|
||||
q = -2 * sqrt(q);
|
||||
x[0] = q * cos(t / 3) - a;
|
||||
x[1] = q * cos((t + TwoPi) / 3) - a;
|
||||
x[2] = q * cos((t - TwoPi) / 3) - a;
|
||||
return (3);
|
||||
}
|
||||
else {
|
||||
//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
|
||||
A = -root3(fabs(r) + sqrt(r2 - q3));
|
||||
if (r < 0)
|
||||
A = -A;
|
||||
B = (A == 0 ? 0 : q / A);
|
||||
|
||||
a /= 3;
|
||||
x[0] = (A + B) - a;
|
||||
x[1] = -0.5 * (A + B) - a;
|
||||
x[2] = 0.5 * sqrt(3.) * (A - B);
|
||||
if (fabs(x[2]) < eps) {
|
||||
x[2] = x[1];
|
||||
return (2);
|
||||
}
|
||||
return (1);
|
||||
}
|
||||
} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
|
||||
{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0
|
||||
btScalar a2 = a * a;
|
||||
btScalar q = (a2 - 3 * b) / 9;
|
||||
if (q < 0)
|
||||
q = eps;
|
||||
btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
|
||||
// equation x^3 + q*x + r = 0
|
||||
btScalar r2 = r * r;
|
||||
btScalar q3 = q * q * q;
|
||||
btScalar A, B;
|
||||
if (r2 <= (q3 + eps))
|
||||
{ //<<-- FIXED!
|
||||
btScalar t = r / sqrt(q3);
|
||||
if (t < -1)
|
||||
t = -1;
|
||||
if (t > 1)
|
||||
t = 1;
|
||||
t = acos(t);
|
||||
a /= 3;
|
||||
q = -2 * sqrt(q);
|
||||
x[0] = q * cos(t / 3) - a;
|
||||
x[1] = q * cos((t + TwoPi) / 3) - a;
|
||||
x[2] = q * cos((t - TwoPi) / 3) - a;
|
||||
return (3);
|
||||
}
|
||||
else
|
||||
{
|
||||
//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
|
||||
A = -root3(fabs(r) + sqrt(r2 - q3));
|
||||
if (r < 0)
|
||||
A = -A;
|
||||
B = (A == 0 ? 0 : q / A);
|
||||
|
||||
a /= 3;
|
||||
x[0] = (A + B) - a;
|
||||
x[1] = -0.5 * (A + B) - a;
|
||||
x[2] = 0.5 * sqrt(3.) * (A - B);
|
||||
if (fabs(x[2]) < eps)
|
||||
{
|
||||
x[2] = x[1];
|
||||
return (2);
|
||||
}
|
||||
return (1);
|
||||
}
|
||||
} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
|
||||
//---------------------------------------------------------------------------
|
||||
// a>=0!
|
||||
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y)
|
||||
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y)
|
||||
{
|
||||
btScalar r = sqrt(x * x + y * y);
|
||||
if (y == 0) {
|
||||
r = sqrt(r);
|
||||
if (x >= 0) {
|
||||
a = r;
|
||||
b = 0;
|
||||
}
|
||||
else {
|
||||
a = 0;
|
||||
b = r;
|
||||
}
|
||||
}
|
||||
else { // y != 0
|
||||
a = sqrt(0.5 * (x + r));
|
||||
b = 0.5 * y / a;
|
||||
}
|
||||
btScalar r = sqrt(x * x + y * y);
|
||||
if (y == 0)
|
||||
{
|
||||
r = sqrt(r);
|
||||
if (x >= 0)
|
||||
{
|
||||
a = r;
|
||||
b = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
a = 0;
|
||||
b = r;
|
||||
}
|
||||
}
|
||||
else
|
||||
{ // y != 0
|
||||
a = sqrt(0.5 * (x + r));
|
||||
b = 0.5 * y / a;
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0
|
||||
int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0
|
||||
{
|
||||
btScalar D = b * b - 4 * d;
|
||||
if (D >= 0) {
|
||||
btScalar sD = sqrt(D);
|
||||
btScalar x1 = (-b + sD) / 2;
|
||||
btScalar x2 = (-b - sD) / 2; // x2 <= x1
|
||||
if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
|
||||
{
|
||||
btScalar sx1 = sqrt(x1);
|
||||
btScalar sx2 = sqrt(x2);
|
||||
x[0] = -sx1;
|
||||
x[1] = sx1;
|
||||
x[2] = -sx2;
|
||||
x[3] = sx2;
|
||||
return 4;
|
||||
}
|
||||
if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
|
||||
{
|
||||
btScalar sx1 = sqrt(-x1);
|
||||
btScalar sx2 = sqrt(-x2);
|
||||
x[0] = 0;
|
||||
x[1] = sx1;
|
||||
x[2] = 0;
|
||||
x[3] = sx2;
|
||||
return 0;
|
||||
}
|
||||
// now x2 < 0 <= x1 , two real roots and one pair of imginary root
|
||||
btScalar sx1 = sqrt(x1);
|
||||
btScalar sx2 = sqrt(-x2);
|
||||
x[0] = -sx1;
|
||||
x[1] = sx1;
|
||||
x[2] = 0;
|
||||
x[3] = sx2;
|
||||
return 2;
|
||||
}
|
||||
else { // if( D < 0 ), two pair of compex roots
|
||||
btScalar sD2 = 0.5 * sqrt(-D);
|
||||
CSqrt(-0.5 * b, sD2, x[0], x[1]);
|
||||
CSqrt(-0.5 * b, -sD2, x[2], x[3]);
|
||||
return 0;
|
||||
} // if( D>=0 )
|
||||
} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d
|
||||
btScalar D = b * b - 4 * d;
|
||||
if (D >= 0)
|
||||
{
|
||||
btScalar sD = sqrt(D);
|
||||
btScalar x1 = (-b + sD) / 2;
|
||||
btScalar x2 = (-b - sD) / 2; // x2 <= x1
|
||||
if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
|
||||
{
|
||||
btScalar sx1 = sqrt(x1);
|
||||
btScalar sx2 = sqrt(x2);
|
||||
x[0] = -sx1;
|
||||
x[1] = sx1;
|
||||
x[2] = -sx2;
|
||||
x[3] = sx2;
|
||||
return 4;
|
||||
}
|
||||
if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
|
||||
{
|
||||
btScalar sx1 = sqrt(-x1);
|
||||
btScalar sx2 = sqrt(-x2);
|
||||
x[0] = 0;
|
||||
x[1] = sx1;
|
||||
x[2] = 0;
|
||||
x[3] = sx2;
|
||||
return 0;
|
||||
}
|
||||
// now x2 < 0 <= x1 , two real roots and one pair of imginary root
|
||||
btScalar sx1 = sqrt(x1);
|
||||
btScalar sx2 = sqrt(-x2);
|
||||
x[0] = -sx1;
|
||||
x[1] = sx1;
|
||||
x[2] = 0;
|
||||
x[3] = sx2;
|
||||
return 2;
|
||||
}
|
||||
else
|
||||
{ // if( D < 0 ), two pair of compex roots
|
||||
btScalar sD2 = 0.5 * sqrt(-D);
|
||||
CSqrt(-0.5 * b, sD2, x[0], x[1]);
|
||||
CSqrt(-0.5 * b, -sD2, x[2], x[3]);
|
||||
return 0;
|
||||
} // if( D>=0 )
|
||||
} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d
|
||||
//---------------------------------------------------------------------------
|
||||
#define SWAP(a, b) \
|
||||
{ \
|
||||
t = b; \
|
||||
b = a; \
|
||||
a = t; \
|
||||
}
|
||||
static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c
|
||||
{ \
|
||||
t = b; \
|
||||
b = a; \
|
||||
a = t; \
|
||||
}
|
||||
static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c
|
||||
{
|
||||
btScalar t;
|
||||
if (a > b)
|
||||
SWAP(a, b); // now a<=b
|
||||
if (c < b) {
|
||||
SWAP(b, c); // now a<=b, b<=c
|
||||
if (a > b)
|
||||
SWAP(a, b); // now a<=b
|
||||
}
|
||||
btScalar t;
|
||||
if (a > b)
|
||||
SWAP(a, b); // now a<=b
|
||||
if (c < b)
|
||||
{
|
||||
SWAP(b, c); // now a<=b, b<=c
|
||||
if (a > b)
|
||||
SWAP(a, b); // now a<=b
|
||||
}
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
|
||||
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
|
||||
{
|
||||
//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
|
||||
if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
|
||||
return SolveP4Bi(x, b, d); // After that, c!=0
|
||||
|
||||
int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
|
||||
// by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
|
||||
if (res3 > 1) // 3 real roots,
|
||||
{
|
||||
dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
|
||||
// Note: x[0]*x[1]*x[2]= c*c > 0
|
||||
if (x[0] > 0) // all roots are positive
|
||||
{
|
||||
btScalar sz1 = sqrt(x[0]);
|
||||
btScalar sz2 = sqrt(x[1]);
|
||||
btScalar sz3 = sqrt(x[2]);
|
||||
// Note: sz1*sz2*sz3= -c (and not equal to 0)
|
||||
if (c > 0) {
|
||||
x[0] = (-sz1 - sz2 - sz3) / 2;
|
||||
x[1] = (-sz1 + sz2 + sz3) / 2;
|
||||
x[2] = (+sz1 - sz2 + sz3) / 2;
|
||||
x[3] = (+sz1 + sz2 - sz3) / 2;
|
||||
return 4;
|
||||
}
|
||||
// now: c<0
|
||||
x[0] = (-sz1 - sz2 + sz3) / 2;
|
||||
x[1] = (-sz1 + sz2 - sz3) / 2;
|
||||
x[2] = (+sz1 - sz2 - sz3) / 2;
|
||||
x[3] = (+sz1 + sz2 + sz3) / 2;
|
||||
return 4;
|
||||
} // if( x[0] > 0) // all roots are positive
|
||||
// now x[0] <= x[1] < 0, x[2] > 0
|
||||
// two pair of comlex roots
|
||||
btScalar sz1 = sqrt(-x[0]);
|
||||
btScalar sz2 = sqrt(-x[1]);
|
||||
btScalar sz3 = sqrt(x[2]);
|
||||
|
||||
if (c > 0) // sign = -1
|
||||
{
|
||||
x[0] = -sz3 / 2;
|
||||
x[1] = (sz1 - sz2) / 2; // x[0]i*x[1]
|
||||
x[2] = sz3 / 2;
|
||||
x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3]
|
||||
return 0;
|
||||
}
|
||||
// now: c<0 , sign = +1
|
||||
x[0] = sz3 / 2;
|
||||
x[1] = (-sz1 + sz2) / 2;
|
||||
x[2] = -sz3 / 2;
|
||||
x[3] = (sz1 + sz2) / 2;
|
||||
return 0;
|
||||
} // if( res3>1 ) // 3 real roots,
|
||||
// now resoventa have 1 real and pair of compex roots
|
||||
// x[0] - real root, and x[0]>0,
|
||||
// x[1]i*x[2] - complex roots,
|
||||
// x[0] must be >=0. But one times x[0]=~ 1e-17, so:
|
||||
if (x[0] < 0)
|
||||
x[0] = 0;
|
||||
btScalar sz1 = sqrt(x[0]);
|
||||
btScalar szr, szi;
|
||||
CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
|
||||
if (c > 0) // sign = -1
|
||||
{
|
||||
x[0] = -sz1 / 2 - szr; // 1st real root
|
||||
x[1] = -sz1 / 2 + szr; // 2nd real root
|
||||
x[2] = sz1 / 2;
|
||||
x[3] = szi;
|
||||
return 2;
|
||||
}
|
||||
// now: c<0 , sign = +1
|
||||
x[0] = sz1 / 2 - szr; // 1st real root
|
||||
x[1] = sz1 / 2 + szr; // 2nd real root
|
||||
x[2] = -sz1 / 2;
|
||||
x[3] = szi;
|
||||
return 2;
|
||||
} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
|
||||
//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
|
||||
if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
|
||||
return SolveP4Bi(x, b, d); // After that, c!=0
|
||||
|
||||
int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
|
||||
// by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
|
||||
if (res3 > 1) // 3 real roots,
|
||||
{
|
||||
dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
|
||||
// Note: x[0]*x[1]*x[2]= c*c > 0
|
||||
if (x[0] > 0) // all roots are positive
|
||||
{
|
||||
btScalar sz1 = sqrt(x[0]);
|
||||
btScalar sz2 = sqrt(x[1]);
|
||||
btScalar sz3 = sqrt(x[2]);
|
||||
// Note: sz1*sz2*sz3= -c (and not equal to 0)
|
||||
if (c > 0)
|
||||
{
|
||||
x[0] = (-sz1 - sz2 - sz3) / 2;
|
||||
x[1] = (-sz1 + sz2 + sz3) / 2;
|
||||
x[2] = (+sz1 - sz2 + sz3) / 2;
|
||||
x[3] = (+sz1 + sz2 - sz3) / 2;
|
||||
return 4;
|
||||
}
|
||||
// now: c<0
|
||||
x[0] = (-sz1 - sz2 + sz3) / 2;
|
||||
x[1] = (-sz1 + sz2 - sz3) / 2;
|
||||
x[2] = (+sz1 - sz2 - sz3) / 2;
|
||||
x[3] = (+sz1 + sz2 + sz3) / 2;
|
||||
return 4;
|
||||
} // if( x[0] > 0) // all roots are positive
|
||||
// now x[0] <= x[1] < 0, x[2] > 0
|
||||
// two pair of comlex roots
|
||||
btScalar sz1 = sqrt(-x[0]);
|
||||
btScalar sz2 = sqrt(-x[1]);
|
||||
btScalar sz3 = sqrt(x[2]);
|
||||
|
||||
if (c > 0) // sign = -1
|
||||
{
|
||||
x[0] = -sz3 / 2;
|
||||
x[1] = (sz1 - sz2) / 2; // x[0]i*x[1]
|
||||
x[2] = sz3 / 2;
|
||||
x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3]
|
||||
return 0;
|
||||
}
|
||||
// now: c<0 , sign = +1
|
||||
x[0] = sz3 / 2;
|
||||
x[1] = (-sz1 + sz2) / 2;
|
||||
x[2] = -sz3 / 2;
|
||||
x[3] = (sz1 + sz2) / 2;
|
||||
return 0;
|
||||
} // if( res3>1 ) // 3 real roots,
|
||||
// now resoventa have 1 real and pair of compex roots
|
||||
// x[0] - real root, and x[0]>0,
|
||||
// x[1]i*x[2] - complex roots,
|
||||
// x[0] must be >=0. But one times x[0]=~ 1e-17, so:
|
||||
if (x[0] < 0)
|
||||
x[0] = 0;
|
||||
btScalar sz1 = sqrt(x[0]);
|
||||
btScalar szr, szi;
|
||||
CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
|
||||
if (c > 0) // sign = -1
|
||||
{
|
||||
x[0] = -sz1 / 2 - szr; // 1st real root
|
||||
x[1] = -sz1 / 2 + szr; // 2nd real root
|
||||
x[2] = sz1 / 2;
|
||||
x[3] = szi;
|
||||
return 2;
|
||||
}
|
||||
// now: c<0 , sign = +1
|
||||
x[0] = sz1 / 2 - szr; // 1st real root
|
||||
x[1] = sz1 / 2 + szr; // 2nd real root
|
||||
x[2] = -sz1 / 2;
|
||||
x[3] = szi;
|
||||
return 2;
|
||||
} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
|
||||
//-----------------------------------------------------------------------------
|
||||
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
|
||||
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
|
||||
{
|
||||
btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
|
||||
if (fxs == 0)
|
||||
return x; //return 1e99; <<-- FIXED!
|
||||
btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
|
||||
return x - fx / fxs;
|
||||
btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
|
||||
if (fxs == 0)
|
||||
return x; //return 1e99; <<-- FIXED!
|
||||
btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
|
||||
return x - fx / fxs;
|
||||
}
|
||||
//-----------------------------------------------------------------------------
|
||||
// x - array of size 4
|
||||
@ -284,136 +298,150 @@ btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // o
|
||||
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
|
||||
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
|
||||
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
|
||||
{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
|
||||
// move to a=0:
|
||||
btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
|
||||
btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
|
||||
btScalar b1 = b - 0.375 * a * a;
|
||||
int res = SolveP4De(x, b1, c1, d1);
|
||||
if (res == 4) {
|
||||
x[0] -= a / 4;
|
||||
x[1] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
x[3] -= a / 4;
|
||||
}
|
||||
else if (res == 2) {
|
||||
x[0] -= a / 4;
|
||||
x[1] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
}
|
||||
else {
|
||||
x[0] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
}
|
||||
// one Newton step for each real root:
|
||||
if (res > 0) {
|
||||
x[0] = N4Step(x[0], a, b, c, d);
|
||||
x[1] = N4Step(x[1], a, b, c, d);
|
||||
}
|
||||
if (res > 2) {
|
||||
x[2] = N4Step(x[2], a, b, c, d);
|
||||
x[3] = N4Step(x[3], a, b, c, d);
|
||||
}
|
||||
return res;
|
||||
{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
|
||||
// move to a=0:
|
||||
btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
|
||||
btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
|
||||
btScalar b1 = b - 0.375 * a * a;
|
||||
int res = SolveP4De(x, b1, c1, d1);
|
||||
if (res == 4)
|
||||
{
|
||||
x[0] -= a / 4;
|
||||
x[1] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
x[3] -= a / 4;
|
||||
}
|
||||
else if (res == 2)
|
||||
{
|
||||
x[0] -= a / 4;
|
||||
x[1] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
x[0] -= a / 4;
|
||||
x[2] -= a / 4;
|
||||
}
|
||||
// one Newton step for each real root:
|
||||
if (res > 0)
|
||||
{
|
||||
x[0] = N4Step(x[0], a, b, c, d);
|
||||
x[1] = N4Step(x[1], a, b, c, d);
|
||||
}
|
||||
if (res > 2)
|
||||
{
|
||||
x[2] = N4Step(x[2], a, b, c, d);
|
||||
x[3] = N4Step(x[3], a, b, c, d);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
//-----------------------------------------------------------------------------
|
||||
#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
|
||||
//-----------------------------------------------------------------------------
|
||||
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
{
|
||||
int cnt;
|
||||
if (fabs(e) < eps)
|
||||
return 0;
|
||||
|
||||
btScalar brd = fabs(a); // brd - border of real roots
|
||||
if (fabs(b) > brd)
|
||||
brd = fabs(b);
|
||||
if (fabs(c) > brd)
|
||||
brd = fabs(c);
|
||||
if (fabs(d) > brd)
|
||||
brd = fabs(d);
|
||||
if (fabs(e) > brd)
|
||||
brd = fabs(e);
|
||||
brd++; // brd - border of real roots
|
||||
|
||||
btScalar x0, f0; // less than root
|
||||
btScalar x1, f1; // greater than root
|
||||
btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
|
||||
btScalar dx = 0;
|
||||
|
||||
if (e < 0) {
|
||||
x0 = 0;
|
||||
x1 = brd;
|
||||
f0 = e;
|
||||
f1 = F5(x1);
|
||||
x2 = 0.01 * brd;
|
||||
} // positive root
|
||||
else {
|
||||
x0 = -brd;
|
||||
x1 = 0;
|
||||
f0 = F5(x0);
|
||||
f1 = e;
|
||||
x2 = -0.01 * brd;
|
||||
} // negative root
|
||||
|
||||
if (fabs(f0) < eps)
|
||||
return x0;
|
||||
if (fabs(f1) < eps)
|
||||
return x1;
|
||||
|
||||
// now x0<x1, f(x0)<0, f(x1)>0
|
||||
// Firstly 10 bisections
|
||||
for (cnt = 0; cnt < 10; cnt++) {
|
||||
x2 = (x0 + x1) / 2; // next point
|
||||
//x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
|
||||
f2 = F5(x2); // f(x2)
|
||||
if (fabs(f2) < eps)
|
||||
return x2;
|
||||
if (f2 > 0) {
|
||||
x1 = x2;
|
||||
f1 = f2;
|
||||
}
|
||||
else {
|
||||
x0 = x2;
|
||||
f0 = f2;
|
||||
}
|
||||
}
|
||||
|
||||
// At each step:
|
||||
// x0<x1, f(x0)<0, f(x1)>0.
|
||||
// x2 - next value
|
||||
// we hope that x0 < x2 < x1, but not necessarily
|
||||
do {
|
||||
if (cnt++ > 50)
|
||||
break;
|
||||
if (x2 <= x0 || x2 >= x1)
|
||||
x2 = (x0 + x1) / 2; // now x0 < x2 < x1
|
||||
f2 = F5(x2); // f(x2)
|
||||
if (fabs(f2) < eps)
|
||||
return x2;
|
||||
if (f2 > 0) {
|
||||
x1 = x2;
|
||||
f1 = f2;
|
||||
}
|
||||
else {
|
||||
x0 = x2;
|
||||
f0 = f2;
|
||||
}
|
||||
f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
|
||||
if (fabs(f2s) < eps) {
|
||||
x2 = 1e99;
|
||||
continue;
|
||||
}
|
||||
dx = f2 / f2s;
|
||||
x2 -= dx;
|
||||
} while (fabs(dx) > eps);
|
||||
return x2;
|
||||
} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
int cnt;
|
||||
if (fabs(e) < eps)
|
||||
return 0;
|
||||
|
||||
btScalar brd = fabs(a); // brd - border of real roots
|
||||
if (fabs(b) > brd)
|
||||
brd = fabs(b);
|
||||
if (fabs(c) > brd)
|
||||
brd = fabs(c);
|
||||
if (fabs(d) > brd)
|
||||
brd = fabs(d);
|
||||
if (fabs(e) > brd)
|
||||
brd = fabs(e);
|
||||
brd++; // brd - border of real roots
|
||||
|
||||
btScalar x0, f0; // less than root
|
||||
btScalar x1, f1; // greater than root
|
||||
btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
|
||||
btScalar dx = 0;
|
||||
|
||||
if (e < 0)
|
||||
{
|
||||
x0 = 0;
|
||||
x1 = brd;
|
||||
f0 = e;
|
||||
f1 = F5(x1);
|
||||
x2 = 0.01 * brd;
|
||||
} // positive root
|
||||
else
|
||||
{
|
||||
x0 = -brd;
|
||||
x1 = 0;
|
||||
f0 = F5(x0);
|
||||
f1 = e;
|
||||
x2 = -0.01 * brd;
|
||||
} // negative root
|
||||
|
||||
if (fabs(f0) < eps)
|
||||
return x0;
|
||||
if (fabs(f1) < eps)
|
||||
return x1;
|
||||
|
||||
// now x0<x1, f(x0)<0, f(x1)>0
|
||||
// Firstly 10 bisections
|
||||
for (cnt = 0; cnt < 10; cnt++)
|
||||
{
|
||||
x2 = (x0 + x1) / 2; // next point
|
||||
//x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
|
||||
f2 = F5(x2); // f(x2)
|
||||
if (fabs(f2) < eps)
|
||||
return x2;
|
||||
if (f2 > 0)
|
||||
{
|
||||
x1 = x2;
|
||||
f1 = f2;
|
||||
}
|
||||
else
|
||||
{
|
||||
x0 = x2;
|
||||
f0 = f2;
|
||||
}
|
||||
}
|
||||
|
||||
// At each step:
|
||||
// x0<x1, f(x0)<0, f(x1)>0.
|
||||
// x2 - next value
|
||||
// we hope that x0 < x2 < x1, but not necessarily
|
||||
do
|
||||
{
|
||||
if (cnt++ > 50)
|
||||
break;
|
||||
if (x2 <= x0 || x2 >= x1)
|
||||
x2 = (x0 + x1) / 2; // now x0 < x2 < x1
|
||||
f2 = F5(x2); // f(x2)
|
||||
if (fabs(f2) < eps)
|
||||
return x2;
|
||||
if (f2 > 0)
|
||||
{
|
||||
x1 = x2;
|
||||
f1 = f2;
|
||||
}
|
||||
else
|
||||
{
|
||||
x0 = x2;
|
||||
f0 = f2;
|
||||
}
|
||||
f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
|
||||
if (fabs(f2s) < eps)
|
||||
{
|
||||
x2 = 1e99;
|
||||
continue;
|
||||
}
|
||||
dx = f2 / f2s;
|
||||
x2 -= dx;
|
||||
} while (fabs(dx) > eps);
|
||||
return x2;
|
||||
} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
//-----------------------------------------------------------------------------
|
||||
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
{
|
||||
btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
|
||||
btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
|
||||
return 1 + SolveP4(x + 1, a1, b1, c1, d1);
|
||||
} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
|
||||
btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
|
||||
return 1 + SolveP4(x + 1, a1, b1, c1, d1);
|
||||
} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
//-----------------------------------------------------------------------------
|
||||
|
18
thirdparty/bullet/BulletSoftBody/poly34.h
vendored
18
thirdparty/bullet/BulletSoftBody/poly34.h
vendored
@ -8,31 +8,31 @@
|
||||
// x - array of size 2
|
||||
// return 2: 2 real roots x[0], x[1]
|
||||
// return 0: pair of complex roots: x[0]i*x[1]
|
||||
int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0
|
||||
int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0
|
||||
|
||||
// x - array of size 3
|
||||
// return 3: 3 real roots x[0], x[1], x[2]
|
||||
// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
|
||||
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0
|
||||
int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0
|
||||
|
||||
// x - array of size 4
|
||||
// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
|
||||
// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
|
||||
// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
|
||||
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
|
||||
int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
|
||||
|
||||
// x - array of size 5
|
||||
// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
|
||||
// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
|
||||
// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4],
|
||||
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// And some additional functions for internal use.
|
||||
// Your may remove this definitions from here
|
||||
int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0
|
||||
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0
|
||||
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y)
|
||||
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
|
||||
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0
|
||||
int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0
|
||||
void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y)
|
||||
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
|
||||
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
|
||||
#endif
|
||||
|
@ -138,7 +138,7 @@ struct btDebugPtrMagic
|
||||
};
|
||||
};
|
||||
|
||||
void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename)
|
||||
void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename)
|
||||
{
|
||||
if (size == 0)
|
||||
{
|
||||
@ -195,7 +195,7 @@ void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filenam
|
||||
return (ret);
|
||||
}
|
||||
|
||||
void btAlignedFreeInternal(void *ptr, int line, char *filename)
|
||||
void btAlignedFreeInternal(void *ptr, int line, const char *filename)
|
||||
{
|
||||
void *real;
|
||||
|
||||
|
@ -35,9 +35,9 @@ int btDumpMemoryLeaks();
|
||||
#define btAlignedFree(ptr) \
|
||||
btAlignedFreeInternal(ptr, __LINE__, __FILE__)
|
||||
|
||||
void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename);
|
||||
void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename);
|
||||
|
||||
void btAlignedFreeInternal(void* ptr, int line, char* filename);
|
||||
void btAlignedFreeInternal(void* ptr, int line, const char* filename);
|
||||
|
||||
#else
|
||||
void* btAlignedAllocInternal(size_t size, int alignment);
|
||||
|
@ -105,7 +105,7 @@ public:
|
||||
|
||||
Point64 cross(const Point32& b) const
|
||||
{
|
||||
return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
|
||||
return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x);
|
||||
}
|
||||
|
||||
Point64 cross(const Point64& b) const
|
||||
@ -115,7 +115,7 @@ public:
|
||||
|
||||
int64_t dot(const Point32& b) const
|
||||
{
|
||||
return x * b.x + y * b.y + z * b.z;
|
||||
return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z;
|
||||
}
|
||||
|
||||
int64_t dot(const Point64& b) const
|
||||
@ -2673,6 +2673,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
|
||||
}
|
||||
|
||||
vertices.resize(0);
|
||||
original_vertex_index.resize(0);
|
||||
edges.resize(0);
|
||||
faces.resize(0);
|
||||
|
||||
@ -2683,6 +2684,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
|
||||
{
|
||||
btConvexHullInternal::Vertex* v = oldVertices[copied];
|
||||
vertices.push_back(hull.getCoordinates(v));
|
||||
original_vertex_index.push_back(v->point.index);
|
||||
btConvexHullInternal::Edge* firstEdge = v->edges;
|
||||
if (firstEdge)
|
||||
{
|
||||
|
@ -66,6 +66,9 @@ public:
|
||||
// Vertices of the output hull
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
|
||||
// The original vertex index in the input coords array
|
||||
btAlignedObjectArray<int> original_vertex_index;
|
||||
|
||||
// Edges of the output hull
|
||||
btAlignedObjectArray<Edge> edges;
|
||||
|
||||
|
@ -267,7 +267,7 @@ public:
|
||||
std::sort(tuples.begin(), tuples.end());
|
||||
btAlignedObjectArray<int> new_indices;
|
||||
btAlignedObjectArray<btVector3> new_vecs;
|
||||
for (int i = 0; i < tuples.size(); ++i)
|
||||
for (size_t i = 0; i < tuples.size(); ++i)
|
||||
{
|
||||
new_indices.push_back(tuples[i].b);
|
||||
new_vecs.push_back(m_vecs[tuples[i].a]);
|
||||
|
2
thirdparty/bullet/LinearMath/btScalar.h
vendored
2
thirdparty/bullet/LinearMath/btScalar.h
vendored
@ -25,7 +25,7 @@ subject to the following restrictions:
|
||||
#include <float.h>
|
||||
|
||||
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
|
||||
#define BT_BULLET_VERSION 289
|
||||
#define BT_BULLET_VERSION 307
|
||||
|
||||
inline int btGetVersion()
|
||||
{
|
||||
|
6
thirdparty/bullet/LinearMath/btSerializer.h
vendored
6
thirdparty/bullet/LinearMath/btSerializer.h
vendored
@ -479,9 +479,9 @@ public:
|
||||
buffer[8] = 'V';
|
||||
}
|
||||
|
||||
buffer[9] = '2';
|
||||
buffer[10] = '8';
|
||||
buffer[11] = '9';
|
||||
buffer[9] = '3';
|
||||
buffer[10] = '0';
|
||||
buffer[11] = '7';
|
||||
}
|
||||
|
||||
virtual void startSerialization()
|
||||
|
Loading…
Reference in New Issue
Block a user