/**
This method makes it possible to evaluate the debug pose at any phase angle
*/
void SimBiController::updateTrackingPose(DynamicArray<double>& trackingPose, double phiToUse){
if( phiToUse < 0 )
phiToUse = phi;
if( phiToUse > 1 )
phiToUse = 1;
trackingPose.clear();
this->character->getState(&trackingPose);
ReducedCharacterState debugRS(&trackingPose);
//always start from a neutral desired pose, and build from there...
for (int i=0;i<jointCount;i++){
debugRS.setJointRelativeOrientation(Quaternion(1, 0, 0, 0), i);
debugRS.setJointRelativeAngVelocity(Vector3d(), i);
controlParams[i].relToCharFrame = false;
}
//and this is the desired orientation for the root
Quaternion qRootD(1, 0, 0, 0);
SimBiConState* curState = states[FSMStateIndex];
for (int i=0;i<curState->getTrajectoryCount();i++){
//now we have the desired rotation angle and axis, so we need to see which joint this is intended for
int jIndex = curState->sTraj[i]->getJointIndex(stance);
//if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error
if (curState->sTraj[i]->relToCharFrame == true || jIndex == swingHipIndex)
controlParams[jIndex].relToCharFrame = true;
Vector3d d0, v0;
computeD0( phiToUse, &d0 );
computeV0( phiToUse, &v0 );
Quaternion newOrientation = curState->sTraj[i]->evaluateTrajectory(this, character->getJoint(jIndex), stance, phiToUse, d - d0, v - v0);
if (jIndex == -1){
qRootD = newOrientation;
}else{
debugRS.setJointRelativeOrientation(newOrientation, jIndex);
}
}
debugRS.setOrientation(qRootD);
//now, we'll make one more pass, and make sure that the orientations that are relative to the character frame are drawn that way
for (int i=0;i<jointCount;i++){
if (controlParams[i].relToCharFrame){
Quaternion temp;
Joint* j = character->getJoint(i);
ArticulatedRigidBody* parent = j->getParent();
while (parent != root){
j = parent->getParentJoint();
parent = j->getParent();
temp = debugRS.getJointRelativeOrientation(character->getJointIndex(j->name)) * temp;
}
temp = qRootD * temp;
temp = temp.getComplexConjugate() * debugRS.getJointRelativeOrientation(i);
debugRS.setJointRelativeOrientation(temp, i);
}
}
}
SimBiController::SimBiController(Character* b) : PoseController(b){
if (b == NULL)
throwError("Cannot create a SIMBICON controller if there is no associated biped!!");
//characters controlled by a simbicon controller are assumed to have: 2 feet
lFoot = b->getARBByName("lFoot");
rFoot = b->getARBByName("rFoot");
if (rFoot == NULL || lFoot == NULL){
lFoot = b->getARBByName("lFoot2");
rFoot = b->getARBByName("rFoot2");
}
if (rFoot == NULL || lFoot == NULL)
throwError("The biped must have the rigid bodies lFoot and rFoot!");
//and two hips connected to the root
Joint* lHip = b->getJointByName("lHip");
Joint* rHip = b->getJointByName("rHip");
lHipIndex = b->getJointIndex("lHip");
rHipIndex = b->getJointIndex("rHip");
if (rFoot == NULL || lFoot == NULL)
throwError("The biped must have the joints lHip and rHip!");
root = b->getRoot();
if (lHip->getParent() != rHip->getParent() || lHip->getParent() != root)
throwError("The biped's hips must have a common parent, which should be the root of the figure!");
setStance(LEFT_STANCE);
phi = 0;
setFSMStateTo(-1);
stanceHipDamping = -1;
stanceHipMaxVelocity = 4;
rootPredictiveTorqueScale = 0;
bodyTouchedTheGround = false;
startingState = -1;
startingStance = LEFT_STANCE;
initialBipState[0] = '\0';
}
/**
This method is used to compute the desired orientation and angular velocity for a parent RB and a child RB, relative to the grandparent RB and
parent RB repsectively. The input is:
- the index of the joint that connects the grandparent RB to the parent RB, and the index of the joint between parent and child
- the distance from the parent's joint with its parent to the location of the child's joint, expressed in parent coordinates
- two rotation normals - one that specifies the plane of rotation for the parent's joint, expressed in grandparent coords,
and the other specifies the plane of rotation between the parent and the child, expressed in parent's coordinates.
- The position of the end effector, expressed in child's coordinate frame
- The desired position of the end effector, expressed in world coordinates
- an estimate of the desired position of the end effector, in world coordinates, some dt later - used to compute desired angular velocities
*/
void IKVMCController::computeIKQandW(int parentJIndex, int childJIndex, const Vector3d& parentAxis, const Vector3d& parentNormal, const Vector3d& childNormal, const Vector3d& childEndEffector, const Point3d& wP, bool computeAngVelocities, const Point3d& futureWP, double dt) {
//this is the joint between the grandparent RB and the parent
Joint* parentJoint = character->joints[parentJIndex];
//this is the grandparent - most calculations will be done in its coordinate frame
ArticulatedRigidBody* gParent = parentJoint->getParent();
//this is the reduced character space where we will be setting the desired orientations and ang vels.
ReducedCharacterState rs(&desiredPose);
//the desired relative orientation between parent and grandparent
Quaternion qParent;
//and the desired relative orientation between child and parent
Quaternion qChild;
TwoLinkIK::getIKOrientations(parentJoint->getParentJointPosition(), gParent->getLocalCoordinates(wP), parentNormal, parentAxis, childNormal, childEndEffector, &qParent, &qChild);
controlParams[parentJIndex].relToFrame = false;
controlParams[childJIndex].relToFrame = false;
rs.setJointRelativeOrientation(qChild, childJIndex);
rs.setJointRelativeOrientation(qParent, parentJIndex);
Vector3d wParentD(0, 0, 0);
Vector3d wChildD(0, 0, 0);
if (computeAngVelocities) {
//the joint's origin will also move, so take that into account, by subbing the offset by which it moves to the
//futureTarget (to get the same relative position to the hip)
Vector3d velOffset = gParent->getAbsoluteVelocityForLocalPoint(parentJoint->getParentJointPosition());
Quaternion qParentF;
Quaternion qChildF;
TwoLinkIK::getIKOrientations(parentJoint->getParentJointPosition(), gParent->getLocalCoordinates(futureWP + velOffset * -dt), parentNormal, parentAxis, childNormal, childEndEffector, &qParentF, &qChildF);
Quaternion qDiff = qParentF * qParent.getComplexConjugate();
wParentD = qDiff.v * 2 / dt;
//the above is the desired angular velocity, were the parent not rotating already - but it usually is, so we'll account for that
wParentD -= gParent->getLocalCoordinates(gParent->getAngularVelocity());
qDiff = qChildF * qChild.getComplexConjugate();
wChildD = qDiff.v * 2 / dt;
//make sure we don't go overboard with the estimates, in case there are discontinuities in the trajectories...
boundToRange(&wChildD.x, -5, 5);
boundToRange(&wChildD.y, -5, 5);
boundToRange(&wChildD.z, -5, 5);
boundToRange(&wParentD.x, -5, 5);
boundToRange(&wParentD.y, -5, 5);
boundToRange(&wParentD.z, -5, 5);
}
rs.setJointRelativeAngVelocity(wChildD, childJIndex);
rs.setJointRelativeAngVelocity(wParentD, parentJIndex);
}
glm::mat4 Joint::getOverallTransformation()
{
Joint* currentParent = getParent();
glm::mat4 overallTransform = this->getLocalTransformation();
while (currentParent != nullptr) {
overallTransform = currentParent->getLocalTransformation() * overallTransform;
currentParent = currentParent->getParent();
}
return overallTransform;
}
/**
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);
}
}
bool Skeleton::isAncestor( unsigned int j1, unsigned int j2 )
{
Joint* joint1 = getJointByIndex( j1 );
Joint* joint2 = getJointByIndex( j2 );
Joint* joint = joint2->getParent();
while( joint )
{
if( joint == joint1 )
{
return true;
}
joint = joint->getParent();
}
return false;
}
bool CCDSolver::compute(std::vector<Joint*>& _links, Vec3f target, bool enableConstraints)
{
/*for(unsigned int i = 0; i < _links.size(); ++i)
{
_links[i]->reset();
}*/
//TO DO
/*
add constraints
do something when the cross product is 0
this happens when the curVector and targetVector are collinears
*/
// start at the last link in the chain
Joint* link = _links[_links.size()-1]->getParent();
Vec3f rootPos = link->getWorldPosition(), curEnd = _links[_links.size()-1]->getWorldPosition(), targetVector, curVector, crossResult, endPos = target;
float cosAngle,turnAngle;
int tries = 0;
float distance = Vec3f(curEnd - endPos).length();
while (++tries < _maxTries &&
distance > _targetThreshold)
{
// create the vector to the current effector pos
curVector = curEnd - rootPos;
// create the desired effector position vector
targetVector = endPos - rootPos;
// normalize the vectors (expensive, requires a sqrt)
curVector.normalize();
targetVector.normalize();
// the dot product gives me the cosine of the desired angle
cosAngle = curVector.dot3(targetVector);
// if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
if (cosAngle < 0.9999999)
{
if(cosAngle < -0.9999999)
{
//the 2 vectors are collinear
//TBD
//
// check if we can use cross product of source vector and [1, 0, 0]
//
crossResult.set(0, curVector.x(), -curVector.y());
if( crossResult.length() < 1.0e-10 )
{
//
// nope! we need cross product of source vector and [0, 1, 0]
//
crossResult.set( -curVector.z(), 0, curVector.x() ) ;
}
}
else
{
// use the cross product to check which way to rotate
crossResult = curVector.cross3(targetVector);
}
crossResult.normalize();
turnAngle = acos(cosAngle); // get the angle
crossResult = link->getWorldRotation().inverse() * crossResult;
Quaternionf rotation;
rotation.setAxisAngle(crossResult, turnAngle);
rotation.normalize();
link->rotate(rotation);
if(enableConstraints)
{
checkDOFsRestrictions(link, link->getDOFConstraints());
}
link->update();
}
if (link->getParent() == NULL)
{
link = _links[_links.size()-1]->getParent(); // start of the chain, restart
}
else
{
link = link->getParent();
}
for(unsigned int i = 0; i < _links.size(); i++ )
{
_links[i]->update();
}
rootPos = link->getWorldPosition();
curEnd = _links[_links.size()-1]->getWorldPosition();
distance = Vec3f(curEnd - endPos).length();
}
if (tries == _maxTries)
{
return false;
}
return true;
}
