diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index ce5980655..33be38cdd 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2844,10 +2844,12 @@ void				btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
 			Node&			b=*l.m_n[1];
 			const btVector3	del=b.m_x-a.m_x;
 			const btScalar	len=del.length2();
-			const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
-			//const btScalar	t=k*a.m_im;
-			a.m_x-=del*(k*a.m_im);
-			b.m_x+=del*(k*b.m_im);
+			if (l.m_c1+len > SIMD_EPSILON)
+			{
+				const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
+				a.m_x-=del*(k*a.m_im);
+				b.m_x+=del*(k*b.m_im);
+			}
 		}
 	}
 }
diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h
index d5716efc3..955bb128e 100644
--- a/src/LinearMath/btAlignedObjectArray.h
+++ b/src/LinearMath/btAlignedObjectArray.h
@@ -406,7 +406,7 @@ class btAlignedObjectArray
 	int	findBinarySearch(const T& key) const
 	{
 		int first = 0;
-		int last = size();
+		int last = size()-1;
 
 		//assume sorted array
 		while (first <= last) {