C++ Joint::getOrientationInParent方法代码示例

/**
 * Convert an OpenSim Joint into the equivalent SIMM joint, only subset of joint types are supported
 * 
*/
void SimbodySimmModel::convertJoint(const Joint& joint)
{

   string parentName;
   string childName;
	bool parentJointAdded = addExtraJoints(joint, parentName, childName);

   SimbodySimmJoint* ssj = new SimbodySimmJoint(joint.getName(), parentName, childName);

	// If parentJointAdded is false, that means the position and orientation in the
	// parent can be merged with the primary joint. So begin making the joint by
	// adding the non-zero components to the SimbodySimmJoint.
	if (parentJointAdded == false) {
		int rotationsSoFar = 0;
		SimTK::Vec3 location;
		SimTK::Vec3 orientation;
		location = joint.getLocationInParent();
		orientation = joint.getOrientationInParent();
		if (NOT_EQUAL_WITHIN_ERROR(location[0], 0.0))
			ssj->addConstantDof("tx", NULL, location[0]);
		if (NOT_EQUAL_WITHIN_ERROR(location[1], 0.0))
			ssj->addConstantDof("ty", NULL, location[1]);
		if (NOT_EQUAL_WITHIN_ERROR(location[2], 0.0))
			ssj->addConstantDof("tz", NULL, location[2]);
		if (NOT_EQUAL_WITHIN_ERROR(orientation[0], 0.0))
			ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[0], orientation[0] * 180.0 / SimTK::Pi);
		if (NOT_EQUAL_WITHIN_ERROR(orientation[1], 0.0))
			ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[1], orientation[1] * 180.0 / SimTK::Pi);
		if (NOT_EQUAL_WITHIN_ERROR(orientation[2], 0.0))
			ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[2], orientation[2] * 180.0 / SimTK::Pi);
	}

	if (joint.getConcreteClassName()==("WeldJoint")) {
		// Nothing to do.
	} else if (joint.getConcreteClassName()==("PinJoint")) {
		int index = 0;
		string coordName = joint.getCoordinateSet().get(index).getName();
		SimTK::Vec3 axis(0.0, 0.0, 1.0); // Pin joints always rotate about the Z axis.
		ssj->addFunctionDof(axis, coordName, 0, Coordinate::Rotational);
	} else if (joint.getConcreteClassName()==("SliderJoint")) {
		int index = 0;
		string coordName = joint.getCoordinateSet().get(index).getName();
		SimTK::Vec3 axis(1.0, 0.0, 0.0); // Slider joints always translate along the X axis.
		ssj->addFunctionDof(axis, coordName, 0, Coordinate::Translational);
	} else if (joint.getConcreteClassName()==("EllipsoidJoint")) {
		// NOTE: Ellipsoid joints cannot be converted into SIMM joints.
	} else if (joint.getConcreteClassName()==("FreeJoint")) {
		SimTK::Vec3 xaxis(1.0, 0.0, 0.0);
		SimTK::Vec3 yaxis(0.0, 1.0, 0.0);
		SimTK::Vec3 zaxis(0.0, 0.0, 1.0);
		int index = 0;
		ssj->addFunctionDof(xaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational);
		ssj->addFunctionDof(yaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational);
		ssj->addFunctionDof(zaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational);
		ssj->addFunctionDof(xaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Rotational);
		ssj->addFunctionDof(yaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Rotational);
		ssj->addFunctionDof(zaxis, joint.getCoordinateSet().get(index).getName(), 0, Coordinate::Rotational);
	} else if (joint.getConcreteClassName()==("CustomJoint")) {
		const CustomJoint* cj = (CustomJoint*)(&joint);
		const CoordinateSet& coordinates = cj->getCoordinateSet();

		// Add the joint's transform axes to the SimbodySimmJoint.
		const SpatialTransform& dofs = cj->getSpatialTransform();
		// Custom joints have the rotational DOFs specified first, but the translations are applied first
		// so you want to process them first here.
		static int order[] = {3, 4, 5, 0, 1, 2};
		for (int i=0; i<6; i++) {
			const TransformAxis* ta = &dofs[order[i]];
			if (ta->getCoordinateNames().size() > 0) { // transform axis is unused if it has no coordinate names
				const Coordinate* coord = NULL;
				const Coordinate* independentCoord = NULL;
				const Function* constraintFunc = NULL;
				Coordinate::MotionType motionType = (order[i]<3) ? Coordinate::Rotational : Coordinate::Translational;
				if (ta->getCoordinateNames().size() > 0)
					coord = &coordinates.get(ta->getCoordinateNames()[0]);
				if (coord)
					constraintFunc = isDependent(coord, &independentCoord);
				if (constraintFunc != NULL) {  // dof is constrained to a coordinate in another joint
					ssj->addFunctionDof(ta->getAxis(), independentCoord->getName(), addJointFunction(constraintFunc, independentCoord->getMotionType(), motionType), motionType);
				} else {
					if (ta->hasFunction())
						constraintFunc = &ta->getFunction();
					if (constraintFunc) // dof is constrained to a coordinate in this joint
						ssj->addFunctionDof(ta->getAxis(), coord->getName(), addJointFunction(constraintFunc, coord->getMotionType(), motionType), motionType);
					else // dof is unconstrained
						ssj->addFunctionDof(ta->getAxis(), coord->getName(), 0, motionType);
				}
			}
		}
	}

	ssj->finalize();
	addSimmJoint(ssj);
}