//==============================================================================
Eigen::Vector6d FreeJoint::getPositionDifferencesStatic(
const Eigen::Vector6d& _q2,
const Eigen::Vector6d& _q1) const
{
const Eigen::Isometry3d T1 = convertToTransform(_q1);
const Eigen::Isometry3d T2 = convertToTransform(_q2);
return convertToPositions(T1.inverse() * T2);
}
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Isometry3d Qnext
= getQ() * convertToTransform(getVelocitiesStatic() * _dt);
setPositionsStatic(convertToPositions(Qnext));
}
//==============================================================================
void EulerJoint::updateLocalTransform() const
{
mT = mJointP.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
* mJointP.mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
//==============================================================================
void EulerJoint::updateRelativeTransform() const
{
mT = Joint::mAspectProperties.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
* Joint::mAspectProperties.mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
//==============================================================================
void FreeJoint::updateLocalTransform() const
{
mQ = convertToTransform(getPositionsStatic());
mT = mT_ParentBodyToJoint * mQ * mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
void getDeltaTransform(AppNode * node, const AppTime & time1, const AppTime & time2, Quaternion & rot, Point3D & trans, Quaternion & srot, Point3D & scale)
{
Matrix<4,4,F32> m1 = node->getNodeTransform(time1);
Matrix<4,4,F32> m2 = node->getNodeTransform(time2);
zapScale(m1);
zapScale(m2);
Matrix<4,4,F32> mat = m1.inverse() * m2;
convertToTransform(mat,rot,trans,srot,scale);
}
void getBlendNodeTransform(AppNode * node, AppNode * parent, AffineParts & child0, AffineParts & parent0, const AppTime & time, const AppTime & referenceTime, Quaternion & rot, Point3D & trans, Quaternion & srot, Point3D & scale)
{
Matrix<4,4,F32> m1, invm1, m2, retMat;
getLocalNodeMatrix(node, parent, referenceTime, m1, child0, parent0);
getLocalNodeMatrix(node, parent, time, m2, child0, parent0);
invm1 = m1.inverse();
retMat = invm1 * m2;
convertToTransform(retMat, rot,trans,srot,scale);
}
//==============================================================================
Eigen::Isometry3d EulerJoint::convertToTransform(
const Eigen::Vector3d &_positions) const
{
return convertToTransform(_positions, getAxisOrder());
}
void getLocalNodeTransform(AppNode * node, AppNode * parent, AffineParts & child0, AffineParts & parent0, const AppTime & time, Quaternion & rot, Point3D & trans, Quaternion & srot, Point3D & scale)
{
Matrix<4,4,F32> localMat;
getLocalNodeMatrix(node,parent,time,localMat,child0,parent0);
convertToTransform(localMat,rot,trans,srot,scale);
}
