Commits

Martin Felis  committed 6fe9757

[matlab] fixed AddBody and AppendBody

  • Participants
  • Parent commits d188481
  • Branches dev

Comments (0)

Files changed (2)

File wrapper/rbdlAddBody.cc

 #include "rbdl_mex.h"
 #include "ObjectHandle.h"
 
+#include <iostream>
 #include <string>
 
 using namespace RigidBodyDynamics;
 	// body
 	
 	Body body;
+	double mass;
+	RigidBodyDynamics::Math::Vector3d com;
+	RigidBodyDynamics::Math::Matrix3d inertia;
 
 	for (int fi = 0; fi < mxGetNumberOfFields (prhs[4]); fi++) {
 		std::string field_name = mxGetFieldNameByNumber(prhs[4], fi);
 				mexErrMsgTxt("Error: number expected for body.mass!");
 				return;
 			}
-			body.mMass =  *mxGetPr(field);
+			mass = *mxGetPr(field);
 		} else if (field_name == "com") {
-			if (!GetVector3d (field, body.mCenterOfMass)) {
+			if (!GetVector3d (field, com)) {
 				mexErrMsgTxt("Error parsing body.com");
 				return;
 			}
 		} else if (field_name == "inertia") {
-			if (!GetMatrix3d (field, body.mInertia)) {
+			if (!GetMatrix3d (field, inertia)) {
 				mexErrMsgTxt("Error parsing body.inertia.");
 				return;
 			}
 		}
 	}
 
+	body = Body (mass, com, inertia);
+
 	// body name (optional)
 	
 	std::string body_name;

File wrapper/rbdlAppendBody.cc

 	// body
 	
 	Body body;
+	double mass;
+	RigidBodyDynamics::Math::Vector3d com;
+	RigidBodyDynamics::Math::Matrix3d inertia;
 
 	for (int fi = 0; fi < mxGetNumberOfFields (prhs[3]); fi++) {
 		std::string field_name = mxGetFieldNameByNumber(prhs[3], fi);
 				mexErrMsgTxt("Error: number expected for body.mass!");
 				return;
 			}
-			body.mMass =  *mxGetPr(field);
+			mass =  *mxGetPr(field);
 		} else if (field_name == "com") {
-			if (!GetVector3d (field, body.mCenterOfMass)) {
+			if (!GetVector3d (field, com)) {
 				mexErrMsgTxt("Error parsing body.com");
 				return;
 			}
 		} else if (field_name == "inertia") {
-			if (!GetMatrix3d (field, body.mInertia)) {
+			if (!GetMatrix3d (field, inertia)) {
 				mexErrMsgTxt("Error parsing body.inertia.");
 				return;
 			}
 		}
 	}
 
+	body = Body (mass, com, inertia);
+
 	// body name (optional)
 	
 	std::string body_name;