Merge pull request #2580 from erwincoumans/master

add --mp4fps=30 command line parameter for ExampleBrowser, various other fixes

examples/CommonInterfaces/CommonGraphicsAppInterface.h

@@ -93,6 +93,7 @@ struct CommonGraphicsApp
 		if (blue)
 			*blue = m_backgroundColorRGB[2];
 	}
+	virtual void setMp4Fps(int fps) {}
 	virtual void setBackgroundColor(float red, float green, float blue)
 	{
 		m_backgroundColorRGB[0] = red;

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -921,6 +921,13 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	m_internalData->m_app = s_app;
 	char* gVideoFileName = 0;
 	args.GetCmdLineArgument("mp4", gVideoFileName);
+	int gVideoFps = 0;
+	args.GetCmdLineArgument("mp4fps", gVideoFps);
+	if (gVideoFps)
+	{
+		simpleApp->setMp4Fps(gVideoFps);
+	}
+
 #ifndef NO_OPENGL3
 	if (gVideoFileName)
 		simpleApp->dumpFramesToVideo(gVideoFileName);

examples/InverseKinematics/InverseKinematicsExample.cpp

@@ -100,7 +100,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
 		jacob->SetJendActive();
 	}
 	jacob->ComputeJacobian(targetaa);  // Set up Jacobian and deltaS vectors
-
+	MatrixRmn AugMat;
 	// Calculate the change in theta values
 	switch (ikMethod)
 	{
@@ -108,7 +108,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
 			jacob->CalcDeltaThetasTranspose();  // Jacobian transpose method
 			break;
 		case IK_DLS:
-			jacob->CalcDeltaThetasDLS();  // Damped least squares method
+			jacob->CalcDeltaThetasDLS(AugMat);  // Damped least squares method
 			break;
 		case IK_DLS_SVD:
 			jacob->CalcDeltaThetasDLSwithSVD();

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -66,6 +66,8 @@ struct SimpleInternalData
 	int m_upAxis;  //y=1 or z=2 is supported
 	int m_customViewPortWidth;
 	int m_customViewPortHeight;
+	int m_mp4Fps;
+
 	SimpleInternalData()
 		: m_fontTextureId(0),
 		  m_largeFontTextureId(0),
@@ -82,7 +84,8 @@ struct SimpleInternalData
 		  m_userPointer(0),
 		  m_upAxis(1),
 		  m_customViewPortWidth(-1),
-		  m_customViewPortHeight(-1)
+		  m_customViewPortHeight(-1),
+		  m_mp4Fps(60)
 	{
 	}
 };
@@ -1089,6 +1092,11 @@ void SimpleOpenGL3App::swapBuffer()
 	m_window->startRendering();
 }
 
+void SimpleOpenGL3App::setMp4Fps(int fps)
+{
+	m_data->m_mp4Fps = fps;
+}
+
 // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
 void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 {
@@ -1100,12 +1108,12 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 
 #ifdef _WIN32
 		sprintf(cmd,
-				"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
+				"ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - "
 				"-threads 0 -y -b:v 50000k   -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s",
-				width, height, mp4FileName);
+			m_data->m_mp4Fps, width, height, mp4FileName);
 
-		//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba   -s %dx%d -i - "
+		//sprintf(cmd, "ffmpeg -r %d -f rawvideo -pix_fmt rgba   -s %dx%d -i - "
-		//		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", width, height, mp4FileName);
+		//		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", m_data->m_mp4Fps, width, height, mp4FileName);
 #else
 
 		sprintf(cmd,

examples/OpenGLWindow/SimpleOpenGL3App.h

@@ -14,6 +14,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
 	class GLPrimitiveRenderer* m_primRenderer;
 	class GLInstancingRenderer* m_instancingRenderer;
 	virtual void setBackgroundColor(float red, float green, float blue);
+	virtual void setMp4Fps(int fps);
 
 	SimpleOpenGL3App(const char* title, int width, int height, bool allowRetina = true, int windowType = 0, int renderDevice = -1, int maxNumObjectCapacity = 128 * 1024, int maxShapeCapacityInBytes = 128 * 1024 * 1024);
 

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -44,6 +44,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
 								   const double* q_current, int numQ, int endEffectorIndex,
 								   double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
 {
+	MatrixRmn AugMat;
 	bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
 
 	Jacobian ikJacobian(useAngularPart, numQ, 1);
@@ -142,12 +143,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
 		case IK2_VEL_DLS:
 			//ikJacobian.CalcDeltaThetasDLS();			// Damped least squares method
 			assert(m_data->m_dampingCoeff.GetLength() == numQ);
-			ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
+			ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
 			break;
 		case IK2_VEL_DLS_WITH_NULLSPACE:
 		case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
 			assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
-			ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
+			ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
 			break;
 		case IK2_DLS_SVD:
 			ikJacobian.CalcDeltaThetasDLSwithSVD();
@@ -193,6 +194,7 @@ bool IKTrajectoryHelper::computeIK2(
 	const double* q_current, int numQ,
 	double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
 {
+	MatrixRmn AugMat;
 	bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
 
 	Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
@@ -250,12 +252,12 @@ bool IKTrajectoryHelper::computeIK2(
 	case IK2_VEL_DLS:
 		//ikJacobian.CalcDeltaThetasDLS();			// Damped least squares method
 		assert(m_data->m_dampingCoeff.GetLength() == numQ);
-		ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
+		ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
 		break;
 	case IK2_VEL_DLS_WITH_NULLSPACE:
 	case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
 		assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
-		ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
+		ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
 		break;
 	case IK2_DLS_SVD:
 		ikJacobian.CalcDeltaThetasDLSwithSVD();

examples/SharedMemory/SharedMemoryCommands.h

@@ -789,22 +789,6 @@ struct CalculateInverseKinematicsResultArgs
 	double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
 };
 
-enum EnumUserConstraintFlags
-{
-	USER_CONSTRAINT_ADD_CONSTRAINT = 1,
-	USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
-	USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
-	USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
-	USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
-	USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
-	USER_CONSTRAINT_REQUEST_INFO = 64,
-	USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
-	USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
-	USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
-	USER_CONSTRAINT_CHANGE_ERP = 1024,
-	USER_CONSTRAINT_REQUEST_STATE = 2048,
-	USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
-};
 
 enum EnumBodyChangeFlags
 {

examples/SharedMemory/SharedMemoryPublic.h

@@ -309,6 +309,23 @@ struct b3UserDataValue
 	const char* m_data1;
 };
 
+enum EnumUserConstraintFlags
+{
+	USER_CONSTRAINT_ADD_CONSTRAINT = 1,
+	USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
+	USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
+	USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
+	USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
+	USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
+	USER_CONSTRAINT_REQUEST_INFO = 64,
+	USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
+	USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
+	USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
+	USER_CONSTRAINT_CHANGE_ERP = 1024,
+	USER_CONSTRAINT_REQUEST_STATE = 2048,
+	USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
+};
+
 struct b3UserConstraint
 {
 	int m_parentBodyIndex;

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -620,7 +620,7 @@ int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, in
 	return -1;
 }
 
-int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo)
+int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3RobotUserConstraint* jointInfo)
 {
 	if (!isConnected())
 	{
@@ -629,16 +629,35 @@ int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3Joi
 	}
 	b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
 
-	if (jointInfo->m_flags & eJointChangeMaxForce)
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
 	{
-		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
+		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_maxAppliedForce);
 	}
 
-	if (jointInfo->m_flags & eJointChangeChildFramePosition)
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
+	{
+		b3InitChangeUserConstraintSetGearRatio(commandHandle, jointInfo->m_gearRatio);
+	}
+
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_ERP)
+	{
+		b3InitChangeUserConstraintSetERP(commandHandle, jointInfo->m_erp);
+	}
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
+	{
+		b3InitChangeUserConstraintSetGearAuxLink(commandHandle, jointInfo->m_gearAuxLink);
+	}
+
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
+	{
+		b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, jointInfo->m_relativePositionTarget);
+	}
+	
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
 	{
 		b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
 	}
-	if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
+	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
 	{
 		b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
 	}
@@ -1131,7 +1150,7 @@ void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double camer
 	}
 }
 
-void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
+void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName)
 {
 	if (!isConnected())
 	{
@@ -1140,10 +1159,16 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
 	}
 
 	b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
-	if (durationInMicroSeconds >= 0)
+
+	if (profileName.length())
 	{
-		b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
+		b3SetProfileTimingType(commandHandle, 0);
 	}
+	else
+	{
+		b3SetProfileTimingType(commandHandle, 1);
+	}
+
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 }
 

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -526,6 +526,100 @@ struct b3RobotSimulatorCreateMultiBodyArgs
 	}
 };
 
+
+struct b3RobotUserConstraint : public b3UserConstraint
+{
+	int m_userUpdateFlags;//see EnumUserConstraintFlags
+
+	void setErp(double erp)
+	{
+		m_erp = erp;
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_ERP;
+	}
+	
+	void setMaxAppliedForce(double maxForce)
+	{
+		m_maxAppliedForce = maxForce;
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE;
+	}
+	
+	void setGearRatio(double gearRatio)
+	{
+		m_gearRatio = gearRatio;
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO;
+	}
+
+	void setGearAuxLink(int link)
+	{
+		m_gearAuxLink = link;
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
+	}
+
+	void setRelativePositionTarget(double target)
+	{
+		m_relativePositionTarget = target;
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET;
+	}
+
+	void setChildPivot(double pivot[3])
+	{
+		m_childFrame[0] = pivot[0];
+		m_childFrame[1] = pivot[1];
+		m_childFrame[2] = pivot[2];
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
+	}
+
+	void setChildFrameOrientation(double orn[4])
+	{
+		m_childFrame[3] = orn[0];
+		m_childFrame[4] = orn[1];
+		m_childFrame[5] = orn[2];
+		m_childFrame[6] = orn[3];
+		m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
+	}
+
+	b3RobotUserConstraint()
+		:m_userUpdateFlags(0)
+	{
+		m_parentBodyIndex = -1;
+		m_parentJointIndex = -1;
+		m_childBodyIndex = -1;
+		m_childJointIndex = -1;
+		//position
+		m_parentFrame[0] = 0;
+		m_parentFrame[1] = 0;
+		m_parentFrame[2] = 0;
+		//orientation quaternion [x,y,z,w]
+		m_parentFrame[3] = 0;
+		m_parentFrame[4] = 0;
+		m_parentFrame[5] = 0;
+		m_parentFrame[6] = 1;
+
+		//position
+		m_childFrame[0] = 0;
+		m_childFrame[1] = 0;
+		m_childFrame[2] = 0;
+		//orientation quaternion [x,y,z,w]
+		m_childFrame[3] = 0;
+		m_childFrame[4] = 0;
+		m_childFrame[5] = 0;
+		m_childFrame[6] = 1;
+
+		m_jointAxis[0] = 0;
+		m_jointAxis[1] = 0;
+		m_jointAxis[2] = 1;
+
+		m_jointType = eFixedType;
+
+		m_maxAppliedForce = 500;
+		m_userConstraintUniqueId = -1;
+		m_gearRatio = -1;
+		m_gearAuxLink = -1;
+		m_relativePositionTarget = 0;
+		m_erp = 0;
+	}
+};
+
 struct b3RobotJointInfo : public b3JointInfo
 {
 	b3RobotJointInfo()
@@ -624,7 +718,7 @@ public:
 
 	int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo);
 
-	int changeConstraint(int constraintId, b3JointInfo *jointInfo);
+	int changeConstraint(int constraintId, b3RobotUserConstraint*jointInfo);
 
 	void removeConstraint(int constraintId);
 
@@ -670,7 +764,7 @@ public:
 	void getVREvents(b3VREventsData *vrEventsData, int deviceTypeFilter);
 	void getKeyboardEvents(b3KeyboardEventsData *keyboardEventsData);
 
-	void submitProfileTiming(const std::string &profileName, int durationInMicroSeconds = 1);
+	void submitProfileTiming(const std::string &profileName);
 
 	// JFC: added these 24 methods
 

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot()
 	m_tree->Compute();
 }
 
-void Jacobian::CalcDeltaThetas()
+void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat)
 {
 	switch (CurrentUpdateMode)
 	{
@@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas()
 			CalcDeltaThetasPseudoinverse();
 			break;
 		case JACOB_DLS:
-			CalcDeltaThetasDLS();
+			CalcDeltaThetasDLS(AugMat);
 			break;
 		case JACOB_SDLS:
 			CalcDeltaThetasSDLS();
@@ -327,7 +327,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
+void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
 	// J.MultiplyTranspose( dTextra, dTheta );
 
 	// Use these two lines for the traditional DLS method
-	U.Solve(dS, &dT1);
+	U.Solve(dS, &dT1, AugMat);
 	J.MultiplyTranspose(dT1, dTheta);
 
 	// Compute JInv in damped least square form
@@ -379,7 +379,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLS()
+void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS()
 	// J.MultiplyTranspose( dTextra, dTheta );
 
 	// Use these two lines for the traditional DLS method
-	U.Solve(dS, &dT1);
+	U.Solve(dS, &dT1, AugMat);
 	J.MultiplyTranspose(dT1, dTheta);
 
 	// Scale back to not exceed maximum angle changes
@@ -404,7 +404,7 @@ void Jacobian::CalcDeltaThetasDLS()
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
+void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
 
 	dT1.SetLength(J.GetNumColumns());
 	J.MultiplyTranspose(dS, dT1);
-	U.Solve(dT1, &dTheta);
+	U.Solve(dT1, &dTheta, AugMat);
 
 	// Scale back to not exceed maximum angle changes
 	double maxChange = dTheta.MaxAbs();

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -66,15 +66,15 @@ public:
 	void SetJendTrans(MatrixRmn& J);
 	void SetDeltaS(VectorRn& S);
 
-	void CalcDeltaThetas();  // Use this only if the Current Mode has been set.
+	void CalcDeltaThetas(MatrixRmn& AugMat);  // Use this only if the Current Mode has been set.
 	void ZeroDeltaThetas();
 	void CalcDeltaThetasTranspose();
 	void CalcDeltaThetasPseudoinverse();
-	void CalcDeltaThetasDLS();
+	void CalcDeltaThetasDLS(MatrixRmn& AugMat);
-	void CalcDeltaThetasDLS2(const VectorRn& dVec);
+	void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat);
 	void CalcDeltaThetasDLSwithSVD();
 	void CalcDeltaThetasSDLS();
-	void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV);
+	void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat);
 
 	void UpdateThetas();
 	void UpdateThetaDot();

examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp

@@ -30,7 +30,6 @@ subject to the following restrictions:
 
 #include "MatrixRmn.h"
 
-MatrixRmn MatrixRmn::WorkMatrix;  // Temporary work matrix
 
 // Fill the diagonal entries with the value d.  The rest of the matrix is unchanged.
 void MatrixRmn::SetDiagonalEntries(double d)
@@ -354,12 +353,13 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B,
 // Solves the equation   (*this)*xVec = b;
 // Uses row operations.  Assumes *this is square and invertible.
 // No error checking for divide by zero or instability (except with asserts)
-void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec) const
+void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const
 {
 	assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
 
 	// Copy this matrix and b into an Augmented Matrix
-	MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1);
+	
+	AugMat.SetSize(NumRows, NumCols + 1);
 	AugMat.LoadAsSubmatrix(*this);
 	AugMat.SetColumn(NumRows, b);
 

examples/ThirdPartyLibs/BussIK/MatrixRmn.h

@@ -117,7 +117,7 @@ public:
 	MatrixRmn& AddToDiagonal(const VectorRn& dVec);
 
 	// Solving systems of linear equations
-	void Solve(const VectorRn& b, VectorRn* x) const;  // Solves the equation   (*this)*x = b;    Uses row operations.  Assumes *this is invertible.
+	void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const;  // Solves the equation   (*this)*x = b;    Uses row operations.  Assumes *this is invertible.
 
 	// Row Echelon Form and Reduced Row Echelon Form routines
 	// Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables.
@@ -150,13 +150,6 @@ private:
 	double* x;       // Array of vector entries - stored in column order
 	long AllocSize;  // Allocated size of the x array
 
-	static MatrixRmn WorkMatrix;  // Temporary work matrix
-	static MatrixRmn& GetWorkMatrix() { return WorkMatrix; }
-	static MatrixRmn& GetWorkMatrix(long numRows, long numCols)
-	{
-		WorkMatrix.SetSize(numRows, numCols);
-		return WorkMatrix;
-	}
 
 	// Internal helper routines for SVD calculations
 	static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);

examples/pybullet/examples/video_sync_mp4.py

@@ -0,0 +1,24 @@
+import pybullet as p
+import time
+
+#by default, PyBullet runs at 240Hz
+p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240")
+p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+p.loadURDF("plane.urdf")
+
+#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter.
+r2d2 = p.loadURDF("r2d2.urdf",[0,0,45])
+#disable linear damping
+p.changeDynamics(r2d2,-1, linearDamping=0)
+p.setGravity(0,0,-10)
+
+for i in range (3*240):
+  txt = "frame "+str(i)
+  item = p.addUserDebugText(txt, [0,1,0])
+  p.stepSimulation()
+  #synchronize the visualizer (rendering frames for the video mp4) with stepSimulation
+  p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+  #print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2])
+  p.removeUserDebugItem(item)
+
+p.disconnect()