1 files changed, 72 insertions, 23 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
index 671e15d314..f91c001f12 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
@@ -19,21 +19,28 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 #include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
 
 class btMultiBodyLinkCollider : public btCollisionObject
 {
-//protected:
+	//protected:
 public:
-
 	btMultiBody* m_multiBody;
 	int m_link;
 
-
-	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
-		:m_multiBody(multiBody),
-		m_link(link)
+	btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
+		: m_multiBody(multiBody),
+		  m_link(link)
 	{
-		m_checkCollideWith =  true;
+		m_checkCollideWith = true;
 		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
 		//this means that some constraints might point to bodies that are not in the islands, causing crashes
 		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
@@ -49,18 +56,18 @@ public:
 	}
 	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
+	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
 	{
 		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
 		if (!other)
@@ -71,47 +78,46 @@ public:
 			return false;
 
 		//check if 'link' has collision disabled
-		if (m_link>=0)
+		if (m_link >= 0)
 		{
 			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
 			{
 				int parent_of_this = m_link;
 				while (1)
 				{
-					if (parent_of_this==-1)
+					if (parent_of_this == -1)
 						break;
 					parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
-					if (parent_of_this==other->m_link)
+					if (parent_of_this == other->m_link)
 					{
 						return false;
 					}
 				}
 			}
-			else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
 			{
-				if ( link.m_parent == other->m_link)
+				if (link.m_parent == other->m_link)
 					return false;
 			}
-
 		}
 
-		if (other->m_link>=0)
+		if (other->m_link >= 0)
 		{
 			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
-			if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
 			{
 				int parent_of_other = other->m_link;
 				while (1)
 				{
-					if (parent_of_other==-1)
+					if (parent_of_other == -1)
 						break;
 					parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
-					if (parent_of_other==this->m_link)
+					if (parent_of_other == this->m_link)
 						return false;
 				}
 			}
-			else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
 			{
 				if (otherLink.m_parent == this->m_link)
 					return false;
@@ -119,7 +125,50 @@ public:
 		}
 		return true;
 	}
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
+};
+
+// clang-format off
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
 };
 
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+// clang-format on
+
+SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData, serializer);
+
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
 
+#endif  //BT_FEATHERSTONE_LINK_COLLIDER_H