diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 03c3442f0..050870ea4 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -851,19 +851,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 			} else
 			{
-
-				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;
-				}
-
-
+				removePickingConstraint();
 			}
 
 			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;
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index cc6b0d6f6..c1e423052 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/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;
diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index ee66193f0..bc371e406 100644
--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -1122,4 +1122,6 @@ void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransf
 	//calculateTransforms();
 }
 
+ 
+
 
diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
index 2bf25443f..c1c5aa851 100644
--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/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;
 
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index fe52995af..7c5e4f6e7 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/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();
+}
\ No newline at end of file
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index 4d35aaa91..4fdac1133 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -452,10 +452,10 @@ public:
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
 
-    void	setLinearUpperLimit(const btVector3& linearUpper)
-    {
-    	m_linearLimits.m_upperLimit = linearUpper;
-    }
+	void	setLinearUpperLimit(const btVector3& linearUpper)
+	{
+		m_linearLimits.m_upperLimit = linearUpper;
+	}
 
 	void	getLinearUpperLimit(btVector3& linearUpper)
 	{
@@ -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)
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
index b5b998a86..d35034566 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+++ b/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();
+}
+
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
index 462f3d1a1..16ff973e4 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
+++ b/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;
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 8431926e4..50e3f73cb 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/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)
 	{
@@ -205,10 +205,13 @@ public:
 		btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
 		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
-	m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
+		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
diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 7d2a50227..e9723a7bb 100755
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/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);
diff --git a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
index 3a4c2afa6..40c56e779 100644
--- a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
+++ b/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();
+}
+
+
diff --git a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
index 4e64a7d7e..a8d404a59 100644
--- a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
+++ b/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);
 };
 
 
diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
index 84ac67eb8..dba41436e 100644
--- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
+++ b/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];
-			btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
-			btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+			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);
diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h
index bb4d97868..d0234a043 100644
--- a/src/LinearMath/btMatrix3x3.h
+++ b/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
 {