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

示例#1

文件： testAssemblySolver.cpp 项目： ANKELA/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]);
    
    if(constraint){
        Vec3 p1inB1, p2inB2, p1inG, p2inG;
        p1inB1 = constraint->get_location_body_1();
        p2inB2 = constraint->get_location_body_2();

        const PhysicalFrame& b1 = constraint->getBody1();
        const PhysicalFrame& b2 = constraint->getBody2();

        p1inG = b1.getTransformInGround(s)*p1inB1;
        p2inG = b2.getTransformInGround(s)*p2inB2;

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

    return error;
}