C++ Joint::getParent方法代码示例

/**
	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)
//.........这里部分代码省略.........