bullet: Sync with upstream 3.07

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 */