/**
This method is used to compute the torques that mimick the effect of applying a force on
a rigid body, at some point. It works best if the end joint is connected to something that
is grounded, otherwise (I think) this is just an approximation.
This function works by making use of the formula:
t = J' * f, where J' is dp/dq, where p is the position where the force is applied, q is
'sorta' the relative orientation between links. It makes the connection between the velocity
of the point p and the relative angular velocities at each joint. Here's an example of how to compute it.
Assume: p = pBase + R1 * v1 + R2 * v2, where R1 is the matrix from link 1 to whatever pBase is specified in,
and R2 is the rotation matrix from link 2 to whatever pBase is specified in, v1 is the point from link 1's
origin to link 2's origin (in link 1 coordinates), and v2 is the vector from origin of link 2 to p
(in link 2 coordinates).
dp/dt = d(R1 * v1)/dt + d(R2 * v2)/dt = d R1/dt * v1 + d R2/dt * v2, and dR/dt = wx * R, where wx is
the cross product matrix associated with the angular velocity w
so dp/dt = w1x * R1 * v1 + w2x * R2 * v2, and w2 = w1 + wRel
= [-(R1*v1 + R2*v2)x -(R2*v1)x ] [w1 wRel]', so the first matrix is the Jacobian.
The first entry is the cross product matrix of the vector (in 'global' coordinates) from the
origin of link 1 to p, and the second entry is the vector (in 'global' coordinates) from
the origin of link 2 to p (and therein lies the general way of writing this).
*/
void VirtualModelController::computeJointTorquesEquivalentToForce(Joint* start, const Point3d& pLocal, const Vector3d& fGlobal, Joint* end){
//starting from the start joint, going towards the end joint, get the origin of each link, in world coordinates,
//and compute the vector to the global coordinates of pLocal.
Joint* currentJoint = start;
Vector3d tmpV;
Point3d pGlobal = start->getChild()->getWorldCoordinates(pLocal);
while (currentJoint != end){
if (currentJoint == NULL)
throwError("VirtualModelController::computeJointTorquesEquivalentToForce --> end was not a parent of start...");
tmpV = Vector3d(currentJoint->getParent()->getWorldCoordinates(currentJoint->getParentJointPosition()), pGlobal);
Vector3d tmpT = tmpV.crossProductWith(fGlobal);
torques[currentJoint->getID()] -= tmpT;
currentJoint = currentJoint->getParent()->getParentJoint();
}
//and we just have to do it once more for the end joint, if it's not NULL
if (end != NULL){
tmpV = Vector3d(currentJoint->getParent()->getWorldCoordinates(currentJoint->getParentJointPosition()), pGlobal);
torques[currentJoint->getID()] -= tmpV.crossProductWith(fGlobal);
}
}
void UpperArmConstraint::runBWDConstraint()
{
if(!(param.direction & BWD))
return;
Joint* jointToConstrain = joint->getBWDJoint() ;
if ( NULL == jointToConstrain )
return ;
//FIND ROTATION(NON-TWISTED BONE)
Position Pjoint_BWD = joint->getBWDJoint()->getPosition();
Position Pjoint_this = joint->getPosition();
Position Pjoint_FWD = joint->getFWDJoint()->getPosition();
Vector3D b_BWD = Vector3D((Pjoint_BWD-Pjoint_this).getX(), (Pjoint_BWD-Pjoint_this).getY(), (Pjoint_BWD-Pjoint_this).getZ());
Vector3D b_FWD = Vector3D((Pjoint_this-Pjoint_FWD).getX(), (Pjoint_this-Pjoint_FWD).getY(), (Pjoint_this-Pjoint_FWD).getZ());
Quaternion Q_FWD_2_BWD = Quaternion::v2q(b_FWD, b_BWD);
Quaternion QW_BWD_ZeroTwist = Q_FWD_2_BWD * (joint->getQwFWD()) ;
//GET ROTATED BONE
Vector3D Initbone_BWD = joint->getFabrik()->getInitBones() [joint->getID()] ;
Vector3D Vbone_BWD = Quaternion::rotVbyQ ( Initbone_BWD, QW_BWD_ZeroTwist ) ;
Vector3D P_BWD = Vector3D ( joint->getPosition() ) + Vbone_BWD ;
jointToConstrain->setPosition ( P_BWD ) ;
//USE LowerArm TO COMPUTE FullTwist
Vector3D Initbone_BWDBWD = joint->getFabrik()->getInitBones() [jointToConstrain->getID()] ;
Vector3D b_BWDBWD = Quaternion::rotVbyQ ( Initbone_BWDBWD, param.q) ;
Float Dot = Vector3D::Dot(Vbone_BWD.getNormalized(), b_BWDBWD.getNormalized());
Quaternion QW_BWD = QW_BWD_ZeroTwist;
//TwistAngle
if( Dot > 0.00000001)
{
Quaternion Q_BWDBWD_2_BWD = Quaternion::v2q(b_BWDBWD, Vbone_BWD);
Quaternion QW_BWD_FullTwist = Q_BWDBWD_2_BWD * param.q ;
float LowerArmWeight = Dot * 0.5 ;
QW_BWD = Quaternion::slerp1(QW_BWD_ZeroTwist, QW_BWD_FullTwist, (1-LowerArmWeight));
Vector3D V_zeroTwist = Quaternion::rotVbyQ(Initbone_BWD.getNormalized(), QW_BWD_ZeroTwist);
Vector3D V_fullTwist = Quaternion::rotVbyQ(Initbone_BWD.getNormalized(), QW_BWD_FullTwist);
// printf("V0: %f %f %f \t V1: %f %f %f \n", V_zeroTwist.getX(),V_zeroTwist.getY(),V_zeroTwist.getZ(),V_fullTwist.getX(),V_fullTwist.getY(),V_fullTwist.getZ());
}
else
{
QW_BWD = QW_BWD_ZeroTwist;
}
joint->setQwBWD(QW_BWD);
jointToConstrain->setQwFWD(QW_BWD);
return;
}