示例#1
0
/**
	This method is used to return the ratio of the weight that is supported by the stance foot.
*/
double SimBiController::getStanceFootWeightRatio(DynamicArray<ContactPoint> *cfs){
	Vector3d stanceFootForce = getForceOnFoot(stanceFoot, cfs);
	Vector3d swingFootForce = getForceOnFoot(swingFoot, cfs);
	double totalYForce = (stanceFootForce + swingFootForce).dotProductWith(SimGlobals::up);

	if (IS_ZERO(totalYForce))
		return -1;
	else
		return stanceFootForce.dotProductWith(SimGlobals::up) / totalYForce;
}
示例#2
0
/**
	Assume that the current quaternion represents the relative orientation between two coordinate frames A and B.
	This method decomposes the current relative rotation into a twist of frame B around the axis v passed in as a
	parameter, and another more arbitrary rotation.

	AqB = AqT * TqB, where T is a frame that is obtained by rotating frame B around the axis v by the angle
	that is returned by this function.

	In the T coordinate frame, v is the same as in B, and AqT is a rotation that aligns v from A to that
	from T.

	It is assumed that vB is a unit vector!! This method returns TqB, which represents a twist about
	the axis vB.
*/
Quaternion Quaternion::decomposeRotation(const Vector3d vB) const{
	//we need to compute v in A's coordinates
	Vector3d vA = this->rotate(vB);
	vA.toUnit();

	double temp = 0;

	//compute the rotation that aligns the vector v in the two coordinate frames (A and T)
	Vector3d rotAxis = vA.crossProductWith(vB);
	rotAxis.toUnit();
	double rotAngle = -safeACOS(vA.dotProductWith(vB));

	Quaternion TqA = Quaternion::getRotationQuaternion(rotAngle, rotAxis*(-1));
	return TqA * (*this);
}
示例#3
0
/**
  * draws the shadows, by projecting the scene on the ground. Only works for planar ground surfaces!
  */
void GLWindow::drawShadows(){
	//this is the normal to the ground surface.
	//it should be changed if the ground isn't flat... For a polygonal mesh, we might have to do this for every polygon. Not fun!
	Vector3d n = Globals::app->getGroundNormal();
	//assume that the point (0, 0, 0) always belongs to the ground. This means that in the equation of the plane, d will always be 0

	//this is the direction of the light.
	Vector3d d = Vector3d(-150,200,200);

	//we'll mark where the ground is by drawing a 1 in the stencil buffer.
	glClear(GL_STENCIL_BUFFER_BIT);
	glEnable(GL_STENCIL_TEST);
	glStencilFunc(GL_ALWAYS, 1, 0xffffffff);
	glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
	Globals::app->drawGround();

	//now we'll render the scene, using the projection matrix, and we'll only be
	//drawing where there is a 1 in the stencil buffer. This makes sure we're only drawing
	//on top of the ground, where it is visible
    glStencilFunc(GL_LESS, 0, 0xffffffff);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);

    //// Draw the shadow
    glPolygonOffset(-2.0f, -2.0f);
    glEnable(GL_POLYGON_OFFSET_FILL);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glColor4f(0.0, 0.0, 0.0, 0.5);

    glPushMatrix();
	double dot = n.dotProductWith(d);

	//this is the projection matrix
	double mat[16] = {
		dot - n.x*d.x,		-n.x * d.y,			-n.x * d.z,			0,
		-n.y * d.x,			dot - n.y * d.y,	-n.y * d.z,			0,
		-n.z * d.x,			- n.z * d.y,		dot - n.z * d.z,	0,
			0,				     0,					0,			   dot
	};

    glMultMatrixd(mat);
	if (Globals::app)
		Globals::app->draw(true);

	glPopMatrix();

    glDisable(GL_POLYGON_OFFSET_FILL);
    glDisable(GL_STENCIL_TEST);
}
示例#4
0
/**
	Assume that the current quaternion represents the relative orientation between two coordinate frames P and C (i.e. q
	rotates vectors from the child/local frame C into the parent/global frame P).

	With v specified in frame C's coordinates, this method decomposes the current relative rotation, such that:

	PqC = qA * qB, where qB represents a rotation about axis v.
	
	This can be thought of us as a twist about axis v - qB - and a more general rotation, and swing
	- qA - decomposition. Note that qB can be thought of as a rotation from the C frame into a tmp trame T,
	and qA a rotation from T into P.

	In the T coordinate frame, v is the same as in C, and qA is a rotation that aligns v from P to that
	from T.
*/
void Quaternion::decomposeRotation(Quaternion* qA, Quaternion* qB, const Vector3d& vC) const{
	//we need to compute v in P's coordinates
	Vector3d vP;
	this->fastRotate(vC, &vP);

	//compute the rotation that alligns the vector v in the two coordinate frames (P and T - remember that v has the same coordinates in C and in T)
	Vector3d rotAxis;
	rotAxis.setToCrossProduct(vP, vC);
	rotAxis.toUnit();
	double rotAngle = -safeACOS(vP.dotProductWith(vC));

	qA->setToRotationQuaternion(rotAngle, rotAxis);
	//now qB = qAinv * PqC
	qB->setToProductOf(*qA, *this, true, false);

	*qB = (*qA).getComplexConjugate() * (*this);
}
示例#5
0
/**
	This method is used to rotate the vector that is passed in as a parameter by the current quaternion (which is assumed to be a 
	unit quaternion).
*/
Vector3d Quaternion::inverseRotate(const Vector3d& u) const{
	//uRot = q * (0, u) * q' = (s, -v) * (0, u) * (s, v)
	//working it out manually, we get:
	Vector3d t = u * s + u.crossProductWith(v);
	return v*u.dotProductWith(v) + t * s + t.crossProductWith(v);
}