From 4b35af38304dac90eb5698a3731ee19cdba93212 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 12 Mar 2020 14:18:16 -0700 Subject: [PATCH] change deformable teleport to absolute pose --- .../PhysicsServerCommandProcessor.cpp | 104 +++++++++--------- src/BulletSoftBody/btSoftBody.cpp | 47 ++++++-- src/BulletSoftBody/btSoftBody.h | 7 +- src/BulletSoftBody/btSoftBodyInternals.h | 20 ++++ src/LinearMath/btMatrix3x3.h | 16 +++ 5 files changed, 134 insertions(+), 60 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 65e972653..844813091 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9137,33 +9137,33 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc } } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - if (body && body->m_softBody) - { - btSoftBody* psb = body->m_softBody; - if (psb) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) - { - psb->forceActivationState(ACTIVE_TAG); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) - { - psb->forceActivationState(DISABLE_DEACTIVATION); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) - { - psb->forceActivationState(ACTIVE_TAG); - psb->setDeactivationTime(0.0); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) - { - psb->forceActivationState(ISLAND_SLEEPING); - } - } - } - } + if (body && body->m_softBody) + { + btSoftBody* psb = body->m_softBody; + if (psb) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) + { + psb->forceActivationState(ACTIVE_TAG); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) + { + psb->forceActivationState(DISABLE_DEACTIVATION); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) + { + psb->forceActivationState(ACTIVE_TAG); + psb->setDeactivationTime(0.0); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) + { + psb->forceActivationState(ISLAND_SLEEPING); + } + } + } + } #endif SharedMemoryStatus& serverCmd = serverStatusOut; @@ -9844,31 +9844,31 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe } } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - if (body && body->m_softBody) - { - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - body->m_softBody->setLinearVelocity(baseLinVel); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - body->m_softBody->setAngularVelocity(baseAngVel); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - btTransform tr; - tr.setIdentity(); - tr.setOrigin(basePos); - body->m_softBody->transform(tr); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - btTransform tr; - tr.setIdentity(); - tr.setRotation(baseOrn); - body->m_softBody->transform(tr); - } - } + if (body && body->m_softBody) + { + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + body->m_softBody->setLinearVelocity(baseLinVel); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + body->m_softBody->setAngularVelocity(baseAngVel); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION || clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + btTransform tr; + tr.setIdentity(); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + tr.setOrigin(basePos); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + tr.setRotation(baseOrn); + } + body->m_softBody->transformTo(tr); + } + } #endif syncPhysicsToGraphics2(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 3ccb1293a..02b1c4815 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" +#include "LinearMath/btImplicitQRSVD.h" #include "LinearMath/btAlignedAllocator.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" @@ -138,6 +139,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV /* Nodes */ const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); + m_X.resize(node_count); for (int i = 0, ni = node_count; i < ni; ++i) { Node& n = m_nodes[i]; @@ -148,6 +150,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); n.m_material = pm; + m_X[i] = n.m_x; } updateBounds(); setCollisionQuadrature(3); @@ -210,7 +213,7 @@ void btSoftBody::initDefaults() m_collisionShape = new btSoftBodyCollisionShape(this); m_collisionShape->setMargin(0.25f); - m_initialWorldTransform.setIdentity(); + m_worldTransform.setIdentity(); m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); @@ -1031,10 +1034,41 @@ void btSoftBody::setAngularVelocity(const btVector3& angVel) } } +// +btTransform btSoftBody::getRigidTransform() +{ + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + for (int i = 0; i < m_nodes.size(); ++i) + { + S += OuterProduct(m_X[i], t-m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U,V; + singularValueDecomposition(S,U,sigma,V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; +} + +// +void btSoftBody::transformTo(const btTransform& trs) +{ + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); +} + // void btSoftBody::transform(const btTransform& trs) { - btVector3 com = getCenterOfMass(); const btScalar margin = getCollisionShape()->getMargin(); ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; @@ -1042,8 +1076,8 @@ void btSoftBody::transform(const btTransform& trs) for (int i = 0, ni = m_nodes.size(); i < ni; ++i) { Node& n = m_nodes[i]; - n.m_x = trs * (n.m_x - com) + com; - n.m_q = trs * (n.m_q - com) + com; + n.m_x = trs * n.m_x; + n.m_q = trs * n.m_q; n.m_n = trs.getBasis() * n.m_n; vol = btDbvtVolume::FromCR(n.m_x, margin); @@ -1052,7 +1086,6 @@ void btSoftBody::transform(const btTransform& trs) updateNormals(); updateBounds(); updateConstants(); - m_initialWorldTransform = trs; } // @@ -3976,8 +4009,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideFace.psb = this; docollideFace.m_colObj1Wrap = pcoWrap; docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = 0.05*(basemargin + timemargin); - docollideFace.stamargin = 0.05*basemargin; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f1206ff70..bef863798 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -809,6 +809,7 @@ public: bool m_useFaceContact; btAlignedObjectArray m_quads; // quadrature points for collision detection btScalar repulsionStiffness; + btAlignedObjectArray m_X; // initial positions btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; @@ -818,7 +819,7 @@ public: btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision - btTransform m_initialWorldTransform; + btTransform m_worldTransform; btVector3 m_windVelocity; @@ -971,6 +972,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); /* Transform */ void transform(const btTransform& trs); /* Translate */ diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index e08f650b6..b1613fd9d 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -938,6 +938,26 @@ static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,con return (m); } +static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2) +{ + btMatrix3x3 m; + btScalar a11 = v1[0] * v2[0]; + btScalar a12 = v1[0] * v2[1]; + btScalar a13 = v1[0] * v2[2]; + + btScalar a21 = v1[1] * v2[0]; + btScalar a22 = v1[1] * v2[1]; + btScalar a23 = v1[1] * v2[2]; + + btScalar a31 = v1[2] * v2[0]; + btScalar a32 = v1[2] * v2[1]; + btScalar a33 = v1[2] * v2[2]; + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + // static inline btMatrix3x3 Add(const btMatrix3x3& a, diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index cc33a6866..9c90fee1d 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -26,10 +26,12 @@ subject to the following restrictions: #endif #if defined(BT_USE_SSE) +#define v0000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f)) #define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f)) #define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f)) #define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f)) #elif defined(BT_USE_NEON) +const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0000) = {0.0f, 0.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f}; const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f}; @@ -330,6 +332,20 @@ public: btScalar(0.0), btScalar(0.0), btScalar(1.0)); #endif } + + /**@brief Set the matrix to the identity */ + void setZero() + { +#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON) + m_el[0] = v0000; + m_el[1] = v0000; + m_el[2] = v0000; +#else + setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(0.0)); +#endif + } static const btMatrix3x3& getIdentity() {