C++ (Cpp) ConstantDistanceConstraint::get_body_2示例

示例#1

文件： testAssemblySolver.cpp 项目： jryong/opensim-core

double calcLigamentLengthError(const SimTK::State &s, const Model &model)
{
	using namespace SimTK;
	double error = 0;

	ConstantDistanceConstraint* constraint = dynamic_cast<ConstantDistanceConstraint*>(&model.getConstraintSet()[0]);
	const BodySet& bodies = model.getBodySet();
	
	if(constraint){
		Vec3 p1inB1, p2inB2, p1inG, p2inG;
		p1inB1 = constraint->get_location_body_1();
		p2inB2 = constraint->get_location_body_2();

		const OpenSim::Body& b1 = bodies.get(constraint->get_body_1());
		const OpenSim::Body& b2 = bodies.get(constraint->get_body_2());

		model.getSimbodyEngine().getPosition(s, b1, p1inB1, p1inG);
		model.getSimbodyEngine().getPosition(s, b2, p2inB2, p2inG);

		double length = (p2inG-p1inG).norm();
		error = length - constraint->get_constant_distance();
	}

	return error;
}