Added some helper methods for constraints and btMatrix3x3,

Thanks to  francois, See Issue 466

Prevent crash in SoftBodyDemo, when picking constraints and demo-mode switches to next demo.

Demos/OpenGL/DemoApplication.cpp

@@ -851,19 +851,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 			} else
 			{
-
+				removePickingConstraint();
-				if (m_pickConstraint && m_dynamicsWorld)
-				{
-					m_dynamicsWorld->removeConstraint(m_pickConstraint);
-					delete m_pickConstraint;
-					//printf("removed constraint %i",gPickingConstraintId);
-					m_pickConstraint = 0;
-					pickedBody->forceActivationState(ACTIVE_TAG);
-					pickedBody->setDeactivationTime( 0.f );
-					pickedBody = 0;
-				}
-
-
 			}
 
 			break;
@@ -876,6 +864,20 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 }
 
+void DemoApplication::removePickingConstraint()
+{
+	if (m_pickConstraint && m_dynamicsWorld)
+	{
+		m_dynamicsWorld->removeConstraint(m_pickConstraint);
+		delete m_pickConstraint;
+		//printf("removed constraint %i",gPickingConstraintId);
+		m_pickConstraint = 0;
+		pickedBody->forceActivationState(ACTIVE_TAG);
+		pickedBody->setDeactivationTime( 0.f );
+		pickedBody = 0;
+	}
+}
+
 void	DemoApplication::mouseMotionFunc(int x,int y)
 {
 
@@ -1330,6 +1332,8 @@ void DemoApplication::renderme()
 
 void	DemoApplication::clientResetScene()
 {
+	removePickingConstraint();
+
 #ifdef SHOW_NUM_DEEP_PENETRATIONS
 	gNumDeepPenetrationChecks = 0;
 	gNumGjkChecks = 0;

Demos/OpenGL/DemoApplication.h

@@ -55,6 +55,8 @@ protected:
 	///constraint for mouse picking
 	btTypedConstraint*		m_pickConstraint;
 
+	virtual void removePickingConstraint();
+
 	btCollisionShape*	m_shootBoxShape;
 
 	float	m_cameraDistance;

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -1123,3 +1123,5 @@ void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransf
 }
 
  
+
+

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -144,15 +144,6 @@ public:
 
 	void	updateRHS(btScalar	timeStep);
 
-	const btTransform & getFrameOffsetA() const
-	{
-		return m_rbAFrame;
-	}
-
-	const btTransform & getFrameOffsetB() const
-	{
-		return m_rbBFrame;
-	}
 
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -254,8 +245,6 @@ public:
 	}
 	bool isPastSwingLimit() { return m_solveSwingLimit; }
 
-	void setFrames(const btTransform & frameA, const btTransform & frameB);
-
 	void setDamping(btScalar damping) { m_damping = damping; }
 
 	void enableMotor(bool b) { m_bMotorEnabled = b; }
@@ -279,6 +268,20 @@ public:
 	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 	///If no axis is provided, it uses the default axis for this constraint.
 	virtual	void setParam(int num, btScalar value, int axis = -1);
+
+	virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
+
+	const btTransform& getFrameOffsetA() const
+	{
+		return m_rbAFrame;
+	}
+
+	const btTransform& getFrameOffsetB() const
+	{
+		return m_rbBFrame;
+	}
+
+
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
 

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -713,7 +713,7 @@ void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
 }
 
 
-void btGeneric6DofConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_frameInA = frameA;
 	m_frameInB = frameB;
@@ -1047,3 +1047,24 @@ btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 	}
 	return retVal;
 }
+
+ 
+
+void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+{
+	btVector3 zAxis = axis1.normalized();
+	btVector3 yAxis = axis2.normalized();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+	                                xAxis[1], yAxis[1], zAxis[1],
+	                               xAxis[2], yAxis[2], zAxis[2]);
+	
+	// now get constraint frame in local coordinate systems
+	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
+	
+	calculateTransforms();
+}

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -548,6 +548,9 @@ public:
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
 
+	void setAxis( const btVector3& axis1, const btVector3& axis2);
+
+
 	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp

@@ -149,5 +149,24 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
 }
 
 
+void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+{
+	btVector3 zAxis = axis1.normalized();
+	btVector3 yAxis = axis2.normalized();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+                                xAxis[1], yAxis[1], zAxis[1],
+                                xAxis[2], yAxis[2], zAxis[2]);
+
+	// now get constraint frame in local coordinate systems
+	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
+
+  calculateTransforms();
+}
+
 
 

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h

@@ -48,6 +48,9 @@ public:
 	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
 	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
 	void setEquilibriumPoint(int index, btScalar val);
+
+	virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
+
 	virtual void getInfo2 (btConstraintInfo2* info);
 
 	virtual	int	calculateSerializeBufferSize() const;

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -143,17 +143,17 @@ public:
 		return m_rbB;	
 	}
 
-	btTransform & getFrameOffsetA()
+	btTransform& getFrameOffsetA()
 	{
 	return m_rbAFrame;
 	}
 
-	btTransform & getFrameOffsetB()
+	btTransform& getFrameOffsetB()
 	{
 		return m_rbBFrame;
 	}
 
-	void setFrames(const btTransform & frameA, const btTransform & frameB);
+	void setFrames(const btTransform& frameA, const btTransform& frameB);
 	
 	void	setAngularOnly(bool angularOnly)
 	{
@@ -206,9 +206,12 @@ public:
 		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
 		m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
+
 		m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
 										rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
 										rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+		m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
+
 	}
 
 	btScalar	getLowerLimit() const

src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -255,6 +255,15 @@ public:
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
+	void setFrames(const btTransform& frameA, const btTransform& frameB) 
+	{ 
+		m_frameInA=frameA; 
+		m_frameInB=frameB;
+		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		buildJacobian();
+	} 
+
+
 	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 	///If no axis is provided, it uses the default axis for this constraint.
 	virtual	void	setParam(int num, btScalar value, int axis = -1);

src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp

@@ -61,3 +61,27 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
 	setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
 }
 
+void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+{
+  m_axis1 = axis1;
+  m_axis2 = axis2;
+
+	btVector3 zAxis = axis1.normalized();
+	btVector3 yAxis = axis2.normalized();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+                                xAxis[1], yAxis[1], zAxis[1],
+                                xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.setOrigin(m_anchor);
+
+	// now get constraint frame in local coordinate systems
+	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
+
+  calculateTransforms();
+}
+
+

src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h

@@ -52,6 +52,8 @@ public:
 	// limits
 	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
 	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
+
+	void setAxis( const btVector3& axis1, const btVector3& axis2);
 };
 
 

src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp

@@ -160,8 +160,12 @@ void	btSoftRigidDynamicsWorld::debugDrawWorld()
 		for (  i=0;i<this->m_softBodies.size();i++)
 		{
 			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))
+			{
 				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
 				btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+			}
+			
 			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 			{
 				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);

src/LinearMath/btMatrix3x3.h

@@ -110,6 +110,16 @@ public:
 	* Equivilant to this = this * m */
 	btMatrix3x3& operator*=(const btMatrix3x3& m); 
 
+	/** @brief Adds by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this + m */
+	btMatrix3x3& operator+=(const btMatrix3x3& m); 
+
+	/** @brief Substractss by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this - m */
+	btMatrix3x3& operator-=(const btMatrix3x3& m); 
+
 	/** @brief Set from the rotational part of a 4x4 OpenGL matrix
 	*  @param m A pointer to the beginning of the array of scalars*/
 	void setFromOpenGLSubMatrix(const btScalar *m)
@@ -523,6 +533,79 @@ btMatrix3x3::operator*=(const btMatrix3x3& m)
 	return *this;
 }
 
+SIMD_FORCE_INLINE btMatrix3x3& 
+btMatrix3x3::operator+=(const btMatrix3x3& m)
+{
+	setValue(
+		m_el[0][0]+m.m_el[0][0], 
+		m_el[0][1]+m.m_el[0][1],
+		m_el[0][2]+m.m_el[0][2],
+		m_el[1][0]+m.m_el[1][0], 
+		m_el[1][1]+m.m_el[1][1],
+		m_el[1][2]+m.m_el[1][2],
+		m_el[2][0]+m.m_el[2][0], 
+		m_el[2][1]+m.m_el[2][1],
+		m_el[2][2]+m.m_el[2][2]);
+	return *this;
+}
+
+SIMD_FORCE_INLINE btMatrix3x3
+operator*(const btMatrix3x3& m, const btScalar & k)
+{
+	return btMatrix3x3(
+		m[0].x()*k,m[0].y()*k,m[0].z()*k,
+		m[1].x()*k,m[1].y()*k,m[1].z()*k,
+		m[2].x()*k,m[2].y()*k,m[2].z()*k);
+}
+
+ SIMD_FORCE_INLINE btMatrix3x3 
+operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+	return btMatrix3x3(
+	m1[0][0]+m2[0][0], 
+	m1[0][1]+m2[0][1],
+	m1[0][2]+m2[0][2],
+	m1[1][0]+m2[1][0], 
+	m1[1][1]+m2[1][1],
+	m1[1][2]+m2[1][2],
+	m1[2][0]+m2[2][0], 
+	m1[2][1]+m2[2][1],
+	m1[2][2]+m2[2][2]);
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+	return btMatrix3x3(
+	m1[0][0]-m2[0][0], 
+	m1[0][1]-m2[0][1],
+	m1[0][2]-m2[0][2],
+	m1[1][0]-m2[1][0], 
+	m1[1][1]-m2[1][1],
+	m1[1][2]-m2[1][2],
+	m1[2][0]-m2[2][0], 
+	m1[2][1]-m2[2][1],
+	m1[2][2]-m2[2][2]);
+}
+
+
+SIMD_FORCE_INLINE btMatrix3x3& 
+btMatrix3x3::operator-=(const btMatrix3x3& m)
+{
+	setValue(
+	m_el[0][0]-m.m_el[0][0], 
+	m_el[0][1]-m.m_el[0][1],
+	m_el[0][2]-m.m_el[0][2],
+	m_el[1][0]-m.m_el[1][0], 
+	m_el[1][1]-m.m_el[1][1],
+	m_el[1][2]-m.m_el[1][2],
+	m_el[2][0]-m.m_el[2][0], 
+	m_el[2][1]-m.m_el[2][1],
+	m_el[2][2]-m.m_el[2][2]);
+	return *this;
+}
+
+
 SIMD_FORCE_INLINE btScalar 
 btMatrix3x3::determinant() const
 {