Permalink
Fetching contributors…
Cannot retrieve contributors at this time
164 lines (107 sloc) 4.17 KB
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/norm.hpp>
using namespace glm;
#include "quaternion_utils.hpp"
// Returns a quaternion such that q*start = dest
quat RotationBetweenVectors(vec3 start, vec3 dest){
start = normalize(start);
dest = normalize(dest);
float cosTheta = dot(start, dest);
vec3 rotationAxis;
if (cosTheta < -1 + 0.001f){
// special case when vectors in opposite directions :
// there is no "ideal" rotation axis
// So guess one; any will do as long as it's perpendicular to start
// This implementation favors a rotation around the Up axis,
// since it's often what you want to do.
rotationAxis = cross(vec3(0.0f, 0.0f, 1.0f), start);
if (length2(rotationAxis) < 0.01 ) // bad luck, they were parallel, try again!
rotationAxis = cross(vec3(1.0f, 0.0f, 0.0f), start);
rotationAxis = normalize(rotationAxis);
return angleAxis(glm::radians(180.0f), rotationAxis);
}
// Implementation from Stan Melax's Game Programming Gems 1 article
rotationAxis = cross(start, dest);
float s = sqrt( (1+cosTheta)*2 );
float invs = 1 / s;
return quat(
s * 0.5f,
rotationAxis.x * invs,
rotationAxis.y * invs,
rotationAxis.z * invs
);
}
// Returns a quaternion that will make your object looking towards 'direction'.
// Similar to RotationBetweenVectors, but also controls the vertical orientation.
// This assumes that at rest, the object faces +Z.
// Beware, the first parameter is a direction, not the target point !
quat LookAt(vec3 direction, vec3 desiredUp){
if (length2(direction) < 0.0001f )
return quat();
// Recompute desiredUp so that it's perpendicular to the direction
// You can skip that part if you really want to force desiredUp
vec3 right = cross(direction, desiredUp);
desiredUp = cross(right, direction);
// Find the rotation between the front of the object (that we assume towards +Z,
// but this depends on your model) and the desired direction
quat rot1 = RotationBetweenVectors(vec3(0.0f, 0.0f, 1.0f), direction);
// Because of the 1rst rotation, the up is probably completely screwed up.
// Find the rotation between the "up" of the rotated object, and the desired up
vec3 newUp = rot1 * vec3(0.0f, 1.0f, 0.0f);
quat rot2 = RotationBetweenVectors(newUp, desiredUp);
// Apply them
return rot2 * rot1; // remember, in reverse order.
}
// Like SLERP, but forbids rotation greater than maxAngle (in radians)
// In conjunction to LookAt, can make your characters
quat RotateTowards(quat q1, quat q2, float maxAngle){
if( maxAngle < 0.001f ){
// No rotation allowed. Prevent dividing by 0 later.
return q1;
}
float cosTheta = dot(q1, q2);
// q1 and q2 are already equal.
// Force q2 just to be sure
if(cosTheta > 0.9999f){
return q2;
}
// Avoid taking the long path around the sphere
if (cosTheta < 0){
q1 = q1*-1.0f;
cosTheta *= -1.0f;
}
float angle = acos(cosTheta);
// If there is only a 2° difference, and we are allowed 5°,
// then we arrived.
if (angle < maxAngle){
return q2;
}
// This is just like slerp(), but with a custom t
float t = maxAngle / angle;
angle = maxAngle;
quat res = (sin((1.0f - t) * angle) * q1 + sin(t * angle) * q2) / sin(angle);
res = normalize(res);
return res;
}
void tests(){
glm::vec3 Xpos(+1.0f, 0.0f, 0.0f);
glm::vec3 Ypos( 0.0f, +1.0f, 0.0f);
glm::vec3 Zpos( 0.0f, 0.0f, +1.0f);
glm::vec3 Xneg(-1.0f, 0.0f, 0.0f);
glm::vec3 Yneg( 0.0f, -1.0f, 0.0f);
glm::vec3 Zneg( 0.0f, 0.0f, -1.0f);
// Testing standard, easy case
// Must be 90° rotation on X : 0.7 0 0 0.7
quat X90rot = RotationBetweenVectors(Ypos, Zpos);
// Testing with v1 = v2
// Must be identity : 0 0 0 1
quat id = RotationBetweenVectors(Xpos, Xpos);
// Testing with v1 = -v2
// Must be 180° on +/-Y axis : 0 +/-1 0 0
quat Y180rot = RotationBetweenVectors(Xpos, Xneg);
// Testing with v1 = -v2, but with a "bad first guess"
// Must be 180° on +/-Y axis : 0 +/-1 0 0
quat X180rot = RotationBetweenVectors(Zpos, Zneg);
}