diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBody.h')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftBody.h | 244 |
1 files changed, 223 insertions, 21 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h index 2b048c1118..6a55eccbd2 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h @@ -35,6 +35,8 @@ subject to the following restrictions: //#else #define btSoftBodyData btSoftBodyFloatData #define btSoftBodyDataName "btSoftBodyFloatData" +static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; +static unsigned long seed = 243703; //#endif //BT_USE_DOUBLE_PRECISION class btBroadphaseInterface; @@ -161,14 +163,18 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - SDF_RD = 0x0003, ///DF based rigid vs deformable - SDF_RDF = 0x0004, ///DF based rigid vs deformable faces + SDF_RD = 0x0004, ///rigid vs deformable - SVSmask = 0x00F0, ///Rigid versus soft mask + SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling CL_SELF = 0x0040, ///Cluster soft body self collision - VF_DD = 0x0050, ///Vertex vs face soft vs soft handling + 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 /* presets */ Default = SDF_RS, END @@ -257,13 +263,13 @@ public: btVector3 m_x; // Position btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity - btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse 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_battach : 1; // Attached int index; }; @@ -289,6 +295,7 @@ public: 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 */ @@ -717,6 +724,15 @@ public: /* SolverState */ struct SolverState { + //if you add new variables, always initialize them! + SolverState() + :sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) + { + } btScalar sdt; // dt*timescale btScalar isdt; // 1/sdt btScalar velmrg; // velocity margin @@ -796,22 +812,24 @@ public: 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; - bool m_useFaceContact; - btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection - - btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; - btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; - bool m_useSelfCollision; + btScalar m_dampingCoefficient; // Damping Coefficient + btScalar m_sleepingThreshold; + btScalar m_maxSpeedSquared; + btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection + btScalar m_repulsionStiffness; + 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 + bool m_useSelfCollision; + bool m_softSoftCollision; btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision - btTransform m_initialWorldTransform; - btVector3 m_windVelocity; btScalar m_restLengthScale; @@ -843,11 +861,6 @@ public: { m_dampingCoefficient = damping_coeff; } - - void setUseFaceContact(bool useFaceContact) - { - m_useFaceContact = false; - } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) @@ -957,6 +970,16 @@ public: void setVolumeMass(btScalar mass); /* Set volume density (using tetrahedrons) */ void setVolumeDensity(btScalar density); + /* Get the linear velocity of the center of mass */ + btVector3 getLinearVelocity(); + /* Set the linear velocity of the center of mass */ + 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); /* Transform */ void transform(const btTransform& trs); /* Translate */ @@ -1023,6 +1046,11 @@ public: bool rayTest(const btVector3& rayFrom, const btVector3& rayTo, sRayCast& results); + bool rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const; /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ @@ -1120,6 +1148,7 @@ public: int rayTest(const btVector3& rayFrom, const btVector3& rayTo, btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); + 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; @@ -1152,7 +1181,180 @@ public: static void VSolve_Links(btSoftBody* psb, btScalar kst); static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); + void geometricCollisionHandler(btSoftBody* psb); +#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 + if (use_velocity) + { + btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; + vol = btDbvtVolume::FromPoints(points, 2); + vol.Expand(btVector3(pad, pad, pad)); + } + else + { + vol = btDbvtVolume::FromCR(n->m_x, pad); + } + node->volume = vol; + return; + } + else + { + updateNode(node->childs[0], use_velocity, margin); + updateNode(node->childs[1], use_velocity, margin); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + Merge(node->childs[0]->volume, node->childs[1]->volume, vol); + node->volume = vol; + } + } + + 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 + 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; + 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}; + 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}; + vol = btDbvtVolume::FromPoints(points, 3); + } + vol.Expand(btVector3(pad, pad, pad)); + node->volume = vol; + return; + } + else + { + updateFace(node->childs[0], use_velocity, margin); + updateFace(node->childs[1], use_velocity, margin); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + Merge(node->childs[0]->volume, node->childs[1]->volume, vol); + node->volume = vol; + } + } + void updateFaceTree(bool use_velocity, bool margin) + { + if (m_fdbvt.m_root) + updateFace(m_fdbvt.m_root, use_velocity, margin); + if (m_fdbvnt) + updateFace(m_fdbvnt, use_velocity, margin); + } + + template <typename T> + static inline T BaryEval(const T& a, + const T& b, + const T& c, + const btVector3& coord) + { + return (a * coord.x() + b * coord.y() + c * coord.z()); + } + void applyRepulsionForce(btScalar timeStep, bool applySpringForce) + { + btAlignedObjectArray<int> indices; + { + // randomize the order of repulsive force + indices.resize(m_faceNodeContacts.size()); + for (int i = 0; i < m_faceNodeContacts.size(); ++i) + indices[i] = i; +#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) + int i, ni; + + for (i = 0, ni = indices.size(); i < ni; ++i) + { + btSwap(indices[i], indices[NEXTRAND % ni]); + } + } + for (int k = 0; k < m_faceNodeContacts.size(); ++k) + { + int i = indices[k]; + btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; + btSoftBody::Node* node = c.m_node; + btSoftBody::Face* face = c.m_face; + const btVector3& w = c.m_bary; + 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); + + 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 + if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) + continue; + btVector3 vt = vr - vn*n; + btScalar I = 0; + 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; + 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) + { + for (int j = 0; j < 3; ++j) + face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; + } + if (face_penetration >= node_penetration) + { + 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); + 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) + { + 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) + { + node->m_v -= I_tilde * node->m_im * vt; + } + } + } + } virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) |