Merge pull request #2030 from erwincoumans/master

some fixes in inverse dynamics, PyBullet example comparing explicit pd, stable pd control, position control (constraint)

src/BulletCollision/NarrowPhaseCollision/btConvexCast.h

@@ -22,6 +22,19 @@ subject to the following restrictions:
 class btMinkowskiSumShape;
 #include "LinearMath/btIDebugDraw.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#define MAX_EPSILON (SIMD_EPSILON * 10)
+#else
+#define MAX_ITERATIONS 32
+#define MAX_EPSILON btScalar(0.0001)
+#endif
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
+
 /// btConvexCast is an interface for Casting
 class btConvexCast
 {
@@ -44,7 +57,9 @@ public:
 		CastResult()
 			: m_fraction(btScalar(BT_LARGE_FLOAT)),
 			  m_debugDrawer(0),
-			  m_allowedPenetration(btScalar(0))
+			  m_allowedPenetration(btScalar(0)),
+			  m_subSimplexCastMaxIterations(MAX_ITERATIONS),
+			  m_subSimplexCastEpsilon(MAX_EPSILON)
 		{
 		}
 
@@ -57,6 +72,10 @@ public:
 		btScalar m_fraction;  //input and output
 		btIDebugDraw* m_debugDrawer;
 		btScalar m_allowedPenetration;
+		
+		int m_subSimplexCastMaxIterations;
+		btScalar m_subSimplexCastEpsilon;
+
 	};
 
 	/// cast a convex against another convex object

src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -31,8 +31,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
 	(void)simplexSolver;
 
 	btVector3 guessVectors[] = {
-		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
-		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(),
 		btVector3(0, 0, 1),
 		btVector3(0, 1, 0),
 		btVector3(1, 0, 0),

src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -28,13 +28,7 @@ btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, con
 {
 }
 
-///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
-///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
+
 bool btSubsimplexConvexCast::calcTimeOfImpact(
 	const btTransform& fromA,
 	const btTransform& toA,
@@ -60,7 +54,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis()));
 	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis()));
 	v = supVertexA - supVertexB;
-	int maxIter = MAX_ITERATIONS;
+	int maxIter = result.m_subSimplexCastMaxIterations;
 
 	btVector3 n;
 	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
@@ -69,20 +63,12 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 
 	btScalar dist2 = v.length2();
 
-#ifdef BT_USE_DOUBLE_PRECISION
-	btScalar epsilon = SIMD_EPSILON * 10;
-#else
-	//todo: epsilon kept for backward compatibility of unit tests.
-	//will need to digg deeper to make the algorithm more robust
-	//since, a large epsilon can cause an early termination with false
-	//positive results (ray intersections that shouldn't be there)
-	btScalar epsilon = btScalar(0.0001);
-#endif  //BT_USE_DOUBLE_PRECISION
+
 
 	btVector3 w, p;
 	btScalar VdotR;
 
-	while ((dist2 > epsilon) && maxIter--)
+	while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--)
 	{
 		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis()));
 		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis()));

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -764,6 +764,12 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	btVector3 ax1A = trA.getBasis().getColumn(2);
 	btVector3 ax1B = trB.getBasis().getColumn(2);
 	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if (ax1.length2()<SIMD_EPSILON)
+	{
+		factA=0.f;
+		factB=1.f;
+		ax1 = ax1A * factA + ax1B * factB;
+	}
 	ax1.normalize();
 	// fill first 3 rows
 	// we want: velA + wA x relA == velB + wB x relB

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -134,6 +134,15 @@ public:
 		return m_baseCollider;
 	}
 
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
 	btMultiBodyLinkCollider *getLinkCollider(int index)
 	{
 		if (index >= 0 && index < getNumLinks())

src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 			break;
 		case FLOATING:
 			break;
+		case SPHERICAL:
+			break;
 		default:
 			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
@@ -437,6 +439,16 @@ int MultiBodyTree::finalize()
 				rigid_body.m_Jac_JT(1) = 0.0;
 				rigid_body.m_Jac_JT(2) = 0.0;
 				break;
+			case SPHERICAL:
+				// NOTE/TODO: this is not really correct.
+				// the Jacobians should be 3x3 matrices here !
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
 			case FLOATING:
 				// NOTE/TODO: this is not really correct.
 				// the Jacobians should be 3x3 matrices here !

src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			// does not add a degree of freedom
 			// m_num_dofs+=0;
 			break;
+		case SPHERICAL:
+			m_num_dofs += 3;
+			break;
 		case FLOATING:
 			m_num_dofs += 6;
 			break;

src/LinearMath/btConvexHullComputer.cpp

@@ -926,7 +926,7 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const
 		"decb %%bh\n\t"        // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
 		"shll $16, %%ebx\n\t"  // ebx has same sign as difference
 		: "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
-		: "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
+		: "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
 		: "%rdx", "cc");
 	return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
 								   // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)