Merge remote-tracking branch 'bp/master'

examples/pybullet/examples/dumpVrLog.py

@@ -0,0 +1,108 @@
+import time
+import math
+from datetime import datetime
+import struct
+import sys
+import os, fnmatch
+import argparse
+from time import sleep
+
+def readLogFile(filename, verbose = True):
+  f = open(filename, 'rb')
+
+  print('Opened'),
+  print(filename)
+
+  keys = f.readline().decode('utf8').rstrip('\n').split(',')
+  fmt = f.readline().decode('utf8').rstrip('\n')
+
+  # The byte number of one record
+  sz = struct.calcsize(fmt)
+  # The type number of one record
+  ncols = len(fmt)
+
+  if verbose:
+    print('Keys:'),
+    print(keys)
+    print('Format:'),
+    print(fmt)
+    print('Size:'),
+    print(sz)
+    print('Columns:'),
+    print(ncols)
+
+  # Read data
+  wholeFile = f.read()
+  # split by alignment word
+  chunks = wholeFile.split(b'\xaa\xbb')
+  log = list()
+  if verbose:
+  	print("num chunks:")
+  	print(len(chunks))
+  chunkIndex = 0
+  for chunk in chunks:
+    print("len(chunk)=",len(chunk)," sz = ", sz)
+    if len(chunk) == sz:
+      print("chunk #",chunkIndex)
+      chunkIndex=chunkIndex+1
+      values = struct.unpack(fmt, chunk)
+      record = list()
+      for i in range(ncols):
+        record.append(values[i])
+        if verbose:
+        	print("    ",keys[i],"=",values[i])
+
+      log.append(record)
+
+  return log
+
+
+numArgs = len(sys.argv)
+
+print ('Number of arguments:', numArgs, 'arguments.')
+print ('Argument List:', str(sys.argv))
+fileName = "data/example_log_vr.bin"
+
+if (numArgs>1):
+	fileName = sys.argv[1]
+
+print("filename=")
+print(fileName)
+
+verbose = True
+	
+log = readLogFile(fileName,verbose)
+
+# the index of the first integer in the vr log file for packed buttons
+firstPackedButtonIndex = 13
+# the number of packed buttons in one integer
+numGroupedButtons = 10
+# the number of integers for packed buttons
+numPackedButtons = 7
+# the mask to get the button state
+buttonMask = 7
+
+for record in log:
+	# indices of buttons that are down
+	buttonDownIndices = []
+	# indices of buttons that are triggered
+	buttonTriggeredIndices = []
+	# indices of buttons that are released
+	buttonReleasedIndices = []
+	buttonIndex = 0
+	for packedButtonIndex in range(firstPackedButtonIndex, firstPackedButtonIndex+numPackedButtons):
+		for packButtonShift in range(numGroupedButtons):
+			buttonEvent = buttonMask & record[packedButtonIndex]
+			if buttonEvent & 1:
+				buttonDownIndices.append(buttonIndex)
+			elif buttonEvent & 2:
+				buttonTriggeredIndices.append(buttonIndex)
+			elif buttonEvent & 4:
+				buttonReleasedIndices.append(buttonIndex)
+			record[packedButtonIndex] = record[packedButtonIndex] >> 3
+			buttonIndex += 1
+	if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices):
+		print ('timestamp: ', record[1])
+		print ('button is down: ', buttonDownIndices)
+		print ('button is triggered: ', buttonTriggeredIndices)
+		print ('button is released: ', buttonReleasedIndices)

src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -1336,6 +1336,8 @@ const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer
 			memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex;
 			memPtr->m_subPart = m_contiguousNodes[i].m_subPart;
 			memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex;
+			// Fill padding with zeros to appease msan.
+			memset(memPtr->m_pad, 0, sizeof(memPtr->m_pad));
 		}
 		serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]);
 	}

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -18,9 +18,11 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 btCollisionObject::btCollisionObject()
-	:	m_anisotropicFriction(1.f,1.f,1.f),
+	:	m_interpolationLinearVelocity(0.f, 0.f, 0.f),
-	m_hasAnisotropicFriction(false),
+		m_interpolationAngularVelocity(0.f, 0.f, 0.f),
-	m_contactProcessingThreshold(BT_LARGE_FLOAT),
+		m_anisotropicFriction(1.f,1.f,1.f),
+		m_hasAnisotropicFriction(false),
+		m_contactProcessingThreshold(BT_LARGE_FLOAT),
 		m_broadphaseHandle(0),
 		m_collisionShape(0),
 		m_extensionPointer(0),
@@ -48,6 +50,7 @@ btCollisionObject::btCollisionObject()
 		m_updateRevision(0)
 {
 	m_worldTransform.setIdentity();
+	m_interpolationWorldTransform.setIdentity();
 }
 
 btCollisionObject::~btCollisionObject()
@@ -112,6 +115,9 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
 	dataOut->m_checkCollideWith = m_checkCollideWith;
 
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
 	return btCollisionObjectDataName;
 }
 

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -437,6 +437,9 @@ const char*	btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se
 		trimeshData->m_triangleInfoMap = 0;
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3));
+
 	return "btTriangleMeshShapeData";
 }
 

src/BulletCollision/CollisionShapes/btCapsuleShape.h

@@ -164,11 +164,17 @@ SIMD_FORCE_INLINE	int	btCapsuleShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
 
 	shapeData->m_upAxis = m_upAxis;
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btCapsuleShapeData";
 }
 

src/BulletCollision/CollisionShapes/btCollisionShape.cpp

@@ -106,7 +106,10 @@ const char*	btCollisionShape::serialize(void* dataBuffer, btSerializer* serializ
 		serializer->serializeName(name);
 	}
 	shapeData->m_shapeType = m_shapeType;
-	//shapeData->m_padding//??
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
+
 	return "btCollisionShapeData";
 }
 

src/BulletCollision/CollisionShapes/btConeShape.h

@@ -168,11 +168,17 @@ SIMD_FORCE_INLINE	int	btConeShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
-	
+
 	shapeData->m_upIndex = m_coneIndices[1];
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btConeShapeData";
 }
 

src/BulletCollision/CollisionShapes/btConvexHullShape.cpp

@@ -211,7 +211,10 @@ const char*	btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
 		}
 		serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
 	}
-	
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3));
+
 	return "btConvexHullShapeData";
 }
 

src/BulletCollision/CollisionShapes/btConvexInternalShape.h

@@ -172,6 +172,9 @@ SIMD_FORCE_INLINE	const char*	btConvexInternalShape::serialize(void* dataBuffer,
 	m_localScaling.serializeFloat(shapeData->m_localScaling);
 	shapeData->m_collisionMargin = float(m_collisionMargin);
 
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding = 0;
+
 	return "btConvexInternalShapeData";
 }
 

src/BulletCollision/CollisionShapes/btCylinderShape.h

@@ -199,11 +199,17 @@ SIMD_FORCE_INLINE	int	btCylinderShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
 
 	shapeData->m_upAxis = m_upAxis;
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btCylinderShapeData";
 }
 

src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp

@@ -175,7 +175,10 @@ const char*	btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serial
 		}
 		serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
 	}
-	
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
+
 	return "btMultiSphereShapeData";
 }
 

src/BulletCollision/CollisionShapes/btStaticPlaneShape.h

@@ -94,7 +94,13 @@ SIMD_FORCE_INLINE	const char*	btStaticPlaneShape::serialize(void* dataBuffer, bt
 	m_localScaling.serializeFloat(planeData->m_localScaling);
 	m_planeNormal.serializeFloat(planeData->m_planeNormal);
 	planeData->m_planeConstant = float(m_planeConstant);
-		
+
+	// Fill padding with zeros to appease msan.
+	planeData->m_pad[0] = 0;
+	planeData->m_pad[1] = 0;
+	planeData->m_pad[2] = 0;
+	planeData->m_pad[3] = 0;
+
 	return "btStaticPlaneShapeData";
 }
 

src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp

@@ -293,6 +293,9 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad[0] = 0;
+							tmpIndices[gfxindex].m_pad[1] = 0;
 						}
 						serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
 					}
@@ -311,6 +314,8 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad = 0;
 						}
 						serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
 					}
@@ -375,6 +380,8 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 		serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr);
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding));
 
 	m_scaling.serializeFloat(trimeshData->m_scaling);
 	return "btStridingMeshInterfaceData";

src/BulletCollision/CollisionShapes/btTriangleInfoMap.h

@@ -195,6 +195,13 @@ SIMD_FORCE_INLINE	const char*	btTriangleInfoMap::serialize(void* dataBuffer, btS
 		serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]);
 
 	}
+
+	// Fill padding with zeros to appease msan.
+	tmapData->m_padding[0] = 0;
+	tmapData->m_padding[1] = 0;
+	tmapData->m_padding[2] = 0;
+	tmapData->m_padding[3] = 0;
+
 	return "btTriangleInfoMapData";
 }
 

src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -141,6 +141,14 @@ SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSe
 
 	gear->m_ratio = m_ratio;
 
+	// Fill padding with zeros to appease msan.
+#ifndef BT_USE_DOUBLE_PRECISION
+	gear->m_padding[0] = 0;
+	gear->m_padding[1] = 0;
+	gear->m_padding[2] = 0;
+	gear->m_padding[3] = 0;
+#endif
+
 	return btGearConstraintDataName;
 }
 

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -664,6 +664,11 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* da
 
 	dof->m_rotateOrder = m_rotateOrder;
 
+	dof->m_padding1[0] = 0;
+	dof->m_padding1[1] = 0;
+	dof->m_padding1[2] = 0;
+	dof->m_padding1[3] = 0;
+
 	return btGeneric6DofSpring2ConstraintDataName;
 }
 

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -489,6 +489,14 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_relaxationFactor = float(m_relaxationFactor);
 #endif
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	hingeData->m_padding1[0] = 0;
+	hingeData->m_padding1[1] = 0;
+	hingeData->m_padding1[2] = 0;
+	hingeData->m_padding1[3] = 0;
+#endif
+
 	return btHingeConstraintDataName;
 }
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -1511,6 +1511,9 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 
 		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
 
+		// Fill padding with zeros to appease msan.
+		memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
+
 #ifdef BT_USE_DOUBLE_PRECISION
 		const char* structType = "btDynamicsWorldDoubleData";
 #else//BT_USE_DOUBLE_PRECISION

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -507,6 +507,11 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
 	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
+#endif
+
 	return btRigidBodyDataName;
 }
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -2013,5 +2013,10 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 		}
 		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
 
+		// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+		memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
+#endif
+
 		return btMultiBodyDataName;
 }

src/LinearMath/btQuaternion.h

@@ -355,7 +355,15 @@ public:
 	{
 		return btSqrt(length2());
 	}
-
+	btQuaternion& safeNormalize()
+	{
+		btScalar l2 = length2();
+		if (l2>SIMD_EPSILON)
+		{
+			normalize();
+		}
+		return *this;
+	}
   /**@brief Normalize the quaternion 
    * Such that x^2 + y^2 + z^2 +w^2 = 1 */
 	btQuaternion& normalize() 

src/LinearMath/btTransformUtil.h

@@ -47,13 +47,19 @@ public:
 	#ifdef QUATERNION_DERIVATIVE
 		btQuaternion predictedOrn = curTrans.getRotation();
 		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
-		predictedOrn.normalize();
+		predictedOrn.safeNormalize();
 	#else
 		//Exponential map
 		//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
 
 		btVector3 axis;
-		btScalar	fAngle = angvel.length(); 
+		btScalar	fAngle2 = angvel.length2();
+        	btScalar    fAngle = 0;
+        	if (fAngle2>SIMD_EPSILON)
+        	{
+            		fAngle = btSqrt(fAngle2);
+        	}
+
 		//limit the angular motion
 		if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
 		{
@@ -74,9 +80,16 @@ public:
 		btQuaternion orn0 = curTrans.getRotation();
 
 		btQuaternion predictedOrn = dorn * orn0;
-		predictedOrn.normalize();
+		predictedOrn.safeNormalize();
 	#endif
-		predictedTransform.setRotation(predictedOrn);
+		if (predictedOrn.length2()>SIMD_EPSILON)
+		{
+			predictedTransform.setRotation(predictedOrn);
+		}
+		else
+		{
+			predictedTransform.setBasis(curTrans.getBasis());
+		}
 	}
 
 	static void	calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)