/**
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;
}
/**
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);
}
/**
* 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);
}
/**
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);
}
/**
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);
}