Program Listing for File PositionBasedRigidBodyDynamics.h
↰ Return to documentation for file (PositionBasedDynamics/PositionBasedRigidBodyDynamics.h)
#ifndef POSITION_BASED_RIGID_BODY_DYNAMICS_H
#define POSITION_BASED_RIGID_BODY_DYNAMICS_H
#include "Common/Common.h"
// ------------------------------------------------------------------------------------
namespace PBD
{
class PositionBasedRigidBodyDynamics
{
// -------------- Position Based Rigid Body Dynamics -----------------------------------------------------
private:
static void computeMatrixK(
const Vector3r &connector,
const Real invMass,
const Vector3r &x,
const Matrix3r &inertiaInverseW,
Matrix3r &K);
static void computeMatrixK(
const Vector3r &connector0,
const Vector3r &connector1,
const Real invMass,
const Vector3r &x,
const Matrix3r &inertiaInverseW,
Matrix3r &K);
static void computeMatrixG(const Quaternionr &q, Eigen::Matrix<Real, 4, 3, Eigen::DontAlign> &G);
static void computeMatrixQ(const Quaternionr &q, Eigen::Matrix<Real, 4, 4, Eigen::DontAlign> &Q);
static void computeMatrixQHat(const Quaternionr &q, Eigen::Matrix<Real, 4, 4, Eigen::DontAlign> &Q);
public:
static bool init_BallJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &jointPosition, // position of balljoint
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &jointInfo
);
static bool update_BallJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &ballJointInfo
);
static bool solve_BallJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real,3,4, Eigen::DontAlign> &ballJointInfo, // precomputed ball joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_BallOnLineJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &position, // position of joint
const Vector3r &direction, // direction of line
Eigen::Matrix<Real, 3, 10, Eigen::DontAlign> &jointInfo
);
static bool update_BallOnLineJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 3, 10, Eigen::DontAlign> &jointInfo
);
static bool solve_BallOnLineJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 3, 10, Eigen::DontAlign> &jointInfo, // precomputed joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_HingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &hingeJointPosition, // position of hinge joint
const Vector3r &hingeJointAxis, // axis of hinge joint
Eigen::Matrix<Real, 4, 7, Eigen::DontAlign> &hingeJointInfo
);
static bool update_HingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 7, Eigen::DontAlign> &hingeJointInfo
);
static bool solve_HingeJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 4, 7, Eigen::DontAlign> &hingeJointInfo, // precomputed hinge joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_UniversalJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &jointPosition, // position of universal joint
const Vector3r &jointAxis0, // first axis of universal joint
const Vector3r &jointAxis1, // second axis of universal joint
Eigen::Matrix<Real, 3, 8, Eigen::DontAlign> &jointInfo
);
static bool update_UniversalJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 3, 8, Eigen::DontAlign> &jointInfo
);
static bool solve_UniversalJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 3, 8, Eigen::DontAlign> &jointInfo, // precomputed universal joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_SliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &sliderJointAxis, // axis of slider joint
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool update_SliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool solve_SliderJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo, // precomputed slider joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_TargetPositionMotorSliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &sliderJointAxis, // axis of slider joint
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool update_TargetPositionMotorSliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool solve_TargetPositionMotorSliderJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Real targetPosition, // target position of the servo motor
const Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo, // precomputed slider joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_TargetVelocityMotorSliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &sliderJointAxis, // axis of slider joint
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool update_TargetVelocityMotorSliderJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool solve_TargetVelocityMotorSliderJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo, // precomputed slider joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool velocitySolve_TargetVelocityMotorSliderJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &omega0,
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1,
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &omega1,
const Real targetVelocity, // target velocity of the servo motor
const Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo, // precomputed joint info
Vector3r &corr_v0, Vector3r &corr_omega0,
Vector3r &corr_v1, Vector3r &corr_omega1);
static bool init_TargetAngleMotorHingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &hingeJointPosition, // position of hinge joint
const Vector3r &hingeJointAxis, // axis of hinge joint
Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo
);
static bool update_TargetAngleMotorHingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo
);
static bool solve_TargetAngleMotorHingeJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Real targetAngle, // target angle of the servo motor
const Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo, // precomputed hinge joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_TargetVelocityMotorHingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &hingeJointPosition, // position of hinge joint
const Vector3r &hingeJointAxis, // axis of hinge joint
Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo
);
static bool update_TargetVelocityMotorHingeJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo
);
static bool solve_TargetVelocityMotorHingeJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo, // precomputed hinge joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool velocitySolve_TargetVelocityMotorHingeJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Vector3r &omega0,
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1,
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Vector3r &omega1,
const Real targetAngularVelocity, // target angular velocity of the servo motor
const Eigen::Matrix<Real, 4, 8, Eigen::DontAlign> &jointInfo, // precomputed joint info
Vector3r &corr_v0, Vector3r &corr_omega0,
Vector3r &corr_v1, Vector3r &corr_omega1);
static bool init_RigidBodyParticleBallJoint(
const Vector3r &x0, // center of mass of the rigid body
const Quaternionr &q0, // rotation of the rigid body body
const Vector3r &x1, // position of the particle
Eigen::Matrix<Real, 3, 2, Eigen::DontAlign> &jointInfo
);
static bool update_RigidBodyParticleBallJoint(
const Vector3r &x0, // center of mass of the rigid body
const Quaternionr &q0, // rotation of the rigid body body
const Vector3r &x1, // position of the particle
Eigen::Matrix<Real, 3, 2, Eigen::DontAlign> &jointInfo
);
static bool solve_RigidBodyParticleBallJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if particle is static
const Vector3r &x1, // position of particle
const Eigen::Matrix<Real, 3, 2, Eigen::DontAlign> &jointInfo, // precomputed joint info
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1);
static bool init_RigidBodyContactConstraint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &omega0, // angular velocity of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1, // velocity of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &omega1, // angular velocity of body 1
const Vector3r &cp0, // contact point of body 0
const Vector3r &cp1, // contact point of body 1
const Vector3r &normal, // contact normal in body 1
const Real restitutionCoeff, // coefficient of restitution
Eigen::Matrix<Real, 3, 5, Eigen::DontAlign> &constraintInfo);
static bool velocitySolve_RigidBodyContactConstraint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Vector3r &omega0, // angular velocity of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1, // velocity of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Vector3r &omega1, // angular velocity of body 1
const Real stiffness, // stiffness parameter of penalty impulse
const Real frictionCoeff, // friction coefficient
Real &sum_impulses, // sum of all impulses
Eigen::Matrix<Real, 3, 5, Eigen::DontAlign> &constraintInfo, // precomputed contact info
Vector3r &corr_v0, Vector3r &corr_omega0,
Vector3r &corr_v1, Vector3r &corr_omega1);
static bool init_ParticleRigidBodyContactConstraint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1, // velocity of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &omega1, // angular velocity of body 1
const Vector3r &cp0, // contact point of body 0
const Vector3r &cp1, // contact point of body 1
const Vector3r &normal, // contact normal in body 1
const Real restitutionCoeff, // coefficient of restitution
Eigen::Matrix<Real, 3, 5, Eigen::DontAlign> &constraintInfo);
static bool velocitySolve_ParticleRigidBodyContactConstraint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Vector3r &v0, // velocity of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Vector3r &v1, // velocity of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Vector3r &omega1, // angular velocity of body 1
const Real stiffness, // stiffness parameter of penalty impulse
const Real frictionCoeff, // friction coefficient
Real &sum_impulses, // sum of all impulses
Eigen::Matrix<Real, 3, 5, Eigen::DontAlign> &constraintInfo, // precomputed contact info
Vector3r &corr_v0,
Vector3r &corr_v1, Vector3r &corr_omega1);
static bool init_DistanceJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &pos0,
const Vector3r &pos1,
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &jointInfo
);
static bool update_DistanceJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 3, 4, Eigen::DontAlign> &jointInfo
);
static bool solve_DistanceJoint(
const Real invMass0, // inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Real stiffness,
const Real restLength,
const Real dt,
const Eigen::Matrix<Real,3,4, Eigen::DontAlign> &jointInfo, // precomputed joint info
Real &lambda,
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
static bool init_DamperJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
const Vector3r &direction,
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool update_DamperJoint(
const Vector3r &x0, // center of mass of body 0
const Quaternionr &q0, // rotation of body 0
const Vector3r &x1, // center of mass of body 1
const Quaternionr &q1, // rotation of body 1
Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo
);
static bool solve_DamperJoint(
const Real invMass0, //inverse mass is zero if body is static
const Vector3r &x0, // center of mass of body 0
const Matrix3r &inertiaInverseW0, // inverse inertia tensor (world space) of body 0
const Quaternionr &q0, // rotation of body 0
const Real invMass1, // inverse mass is zero if body is static
const Vector3r &x1, // center of mass of body 1
const Matrix3r &inertiaInverseW1, // inverse inertia tensor (world space) of body 1
const Quaternionr &q1, // rotation of body 1
const Real stiffness,
const Real dt,
const Eigen::Matrix<Real, 4, 6, Eigen::DontAlign> &jointInfo, // precomputed slider joint info
Real &lambda,
Vector3r &corr_x0, Quaternionr &corr_q0,
Vector3r &corr_x1, Quaternionr &corr_q1);
};
}
#endif