summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h255
1 files changed, 136 insertions, 119 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h
index 07c10935f4..d218d96214 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.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 */
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-16 11:56:22 +0000