qv4 eulerToQuaternion(const v3& euler)
{
qv4 pitch = qv4::IDENTITY;
fromAngleAxis(pitch, euler.x, v3::UNIT_X);
qv4 roll = qv4::IDENTITY;
fromAngleAxis(roll, euler.y, v3::UNIT_Y);
qv4 yaw = qv4::IDENTITY;
fromAngleAxis(yaw, euler.z, v3::UNIT_Z);
return (pitch * roll * yaw);
}
qv4 rotationTo(const v3& src,
const v3& dest,
const v3& fallbackAxis)
{
// Based on Stan Melax's article in Game Programming Gems
qv4 q;
// Copy, since cannot modify local
v3 v0 = src;
v3 v1 = dest;
norm(v0);
norm(v1);
f32 d = boost::geometry::dot_product(v0, v1);
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
return qv4::IDENTITY;
}
if (d < (1e-6f - 1.0f))
{
if (! boost::geometry::equals(fallbackAxis, v3::ZERO))
{
// rotate 180 degrees about the fallback axis
fromAngleAxis(q, (f32)M_PI, fallbackAxis);
}
else
{
// Generate an axis
v3 axis = boost::geometry::cross_product(v3::UNIT_X, src);
if (boost::geometry::distance(v3::ZERO, axis) < EPSILON_F) // pick another if colinear
axis = boost::geometry::cross_product(v3::UNIT_Y, src);
norm(axis);
fromAngleAxis(q, (f32)M_PI, axis);
}
}
else
{
f32 s = std::sqrt( (1+d)*2 );
f32 invs = 1 / s;
v3 c = boost::geometry::cross_product(v0, v1);
q.x = c.x * invs;
q.y = c.y * invs;
q.z = c.z * invs;
q.w = s * 0.5f;
norm(q);
}
return q;
}
inline_ MyQuat(const float angle, const Point& axis) // creates a Quat from an Angle axis -- note that if Angle > 360 the resulting rotation is Angle mod 360
{
fromAngleAxis(angle,axis);
}
Quat::Quat(const Radian& rad, const Vec3& vec)
{
fromAngleAxis(rad, vec);
}
// -----------------------------------------------------------------------------------------
void CPepeEngineQuaternion::fromAngleAxis(const float& rfAngle, const CPepeEngineVector3& rkAxis)
{
fromAngleAxis(Radian(rfAngle), rkAxis);
}
Orientation::Orientation(const AngleAxis& angleAxis)
{
fromAngleAxis(angleAxis);
}
/*! \brief This calculates a quaternion from a rotation axis,
where the magnitude of the vector is the angle of rotation.
*/
static Quaternion fromRotationAxis(Vector axis)
{
double angle = axis.nrm();
axis /= angle + (angle==0);
return fromAngleAxis(angle, axis);
}
void Quaternion::fromAngleAxis(const Radian &angle, const Real &nx, const Real &ny, const Real &nz) {
fromAngleAxis(angle, Vector3(nx, ny, nz));
}
