Commits

Martin Felis committed 914d2cb

Allowing joint axes that are non-unit and also allowing joining of a zero inertia body onto another

Comments (0)

Files changed (2)

 	 * original body to the origin of the added body
 	 */
 	void Join (const Math::SpatialTransform &transform, const Body &other_body) {
+		// nothing to do if we join a massles body to the current.
+		if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) {
+			return;
+		}
+
 		double other_mass = other_body.mMass;
 		double new_mass = mMass + other_mass;
 
 
 		if (joint_type == JointTypeRevolute) {
 			// make sure we have a unit axis
-			assert (joint_axis.squaredNorm() == 1.);
-
-			assert ( joint_axis == Math::Vector3d (1., 0., 0.)
-					|| joint_axis == Math::Vector3d (0., 1., 0.)
-					|| joint_axis == Math::Vector3d (0., 0., 1.));
-
 			mJointAxes[0].set (
 					joint_axis[0],
 					joint_axis[1], 
 	bool validate_spatial_axis (Math::SpatialVector &axis) {
 		if (fabs(axis.norm() - 1.0) > 1.0e-8) {
 			std::cerr << "Warning: joint axis is not unit!" << std::endl;
-			return false;
 		}
 
 		bool axis_rotational = false;
 		bool axis_translational = false;
 
 		Math::Vector3d rotation (axis[0], axis[1], axis[2]);
-		if (fabs(rotation.norm() - 1.0) < 1.0e-8)
+		Math::Vector3d translation (axis[3], axis[4], axis[5]);
+
+		if (fabs(translation.norm()) < 1.0e-8)
 			axis_rotational = true;
 
-		Math::Vector3d translation (axis[3], axis[4], axis[5]);
-		if (fabs(translation.norm() - 1.0) < 1.0e-8)
+		if (fabs(rotation.norm()) < 1.0e-8)
 			axis_translational = true;
 
 		return axis_rotational || axis_translational;