From ed3b6973205c7f2c3717c154d799b1f48fa7ddb9 Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com>
Date: Tue, 14 Apr 2020 17:13:00 -0700
Subject: [PATCH] fix rayTest crash for btSoftBody

---
 examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 5 ++++-
 src/BulletSoftBody/btSoftBody.cpp                       | 4 ++++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index e1807e354..5035f0597 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -5986,7 +5986,8 @@ struct BatchRayCaster
 			int linkIndex = -1;
 
 			const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
-			if (body)
+			const btSoftBody* softBody = btSoftBody::upcast(rayResultCallback.m_collisionObject);
+			if (body || softBody)
 			{
 				objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
 			}
@@ -8443,6 +8444,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 			psb->setCollisionFlags(0);
 			psb->setTotalMass(deformable.m_mass);
 			psb->setSelfCollision(useSelfCollision);
+			psb->initializeFaceTree();
 		}
 #endif  //SKIP_DEFORMABLE_BODY
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -8485,6 +8487,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 		*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
 		InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
 		bodyHandle->m_softBody = psb;
+		psb->setUserIndex2(*bodyUniqueId);
 
 		b3VisualShapeData visualShape;
 
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 152035238..883adc99d 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -29,6 +29,10 @@ subject to the following restrictions:
 static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
 {
 	int N = leafNodes.size();
+	if (N == 0)
+	{
+		return NULL;
+	}
 	while (N > 1)
 	{
 		btAlignedObjectArray<bool> marked;