float Transform3D::norm(Transform3D T){
float pos_norm = arma::norm(T.translation());
UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
float angle = q.getAngle();
//TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
UnitQuaternion UnitQuaternion::slerp(const UnitQuaternion& p, const double& t) {
// See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
// Where qa = *this and qb = p
double cosHalfTheta = kW() * p.kW() + kX() * p.kX() + kY() * p.kY() + kZ() * p.kZ();
// If qa=qb or qa=-qb then theta = 0 and we can return qa
if (std::abs(cosHalfTheta) >= 1.0) {
return *this;
}
double halfTheta = utility::math::angle::acos_clamped(cosHalfTheta);
double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// If theta = 180 degrees then result is not fully defined
// We could rotate around any axis normal to qa or qb
if (std::abs(sinHalfTheta) < 0.001) {
return *this * 0.5 + p * 0.5;
}
// Interpolate
double ratioA = std::sin((1 - t) * halfTheta) / sinHalfTheta;
double ratioB = std::sin(t * halfTheta) / sinHalfTheta;
return *this * ratioA + p * ratioB;
}
float Rotation3D::norm(Rotation3D T) {
UnitQuaternion q = UnitQuaternion(T);
// Get angle between -2Pi and 2pi
float angle = q.getAngle();
// Just want magnitude
float theta = std::fabs(angle);
// But rotating more that Pi in one direction is equivalent to a rotation in the other direction
return std::fmin(2 * M_PI - theta, theta);
}
UnitQuaternion&
UnitQuaternion::LnDif(
const UnitQuaternion& p,
const UnitQuaternion& q
)
{
UnitQuaternion r;
r.Divide(q,p);
return(LogN(r));
}
UnitQuaternion UnitQuaternion::operator * (const UnitQuaternion& p) const {
//From http://en.wikipedia.org/wiki/Quaternion#Quaternions_and_the_geometry_of_R3
double realPart = real() * p.real() - arma::dot(imaginary(), p.imaginary());
arma::vec3 imaginaryPart = arma::cross(imaginary(), p.imaginary())
+ p.real() * imaginary()
+ real() * p.imaginary();
return UnitQuaternion(realPart, imaginaryPart);
}
// Testing of UnitQuaternion multiplication
TYPED_TEST (UnitQuaternionsSingleTest, testUnitQuaternionSingleMultiplication) {
typedef typename TestFixture::UnitQuaternion UnitQuaternion;
typedef typename TestFixture::UnitQuaternionScalar UnitQuaternionScalar;
typedef typename TestFixture::EigenQuat EigenQuat;
UnitQuaternion testQuat;
// Check multiplication of two generic quaternions and compare with eigen results
UnitQuaternion quat12 = this->quat1*this->quat2;
EigenQuat eigenQuat12 = this->eigenQuat1*this->eigenQuat2;
ASSERT_EQ(quat12.w(),eigenQuat12.w());
ASSERT_EQ(quat12.x(),eigenQuat12.x());
ASSERT_EQ(quat12.y(),eigenQuat12.y());
ASSERT_EQ(quat12.z(),eigenQuat12.z());
// Check result of multiplication of a generic quaternion with identity
testQuat = this->quat1*this->quatIdentity;
ASSERT_EQ(testQuat.w(),this->quat1.w());
ASSERT_EQ(testQuat.x(),this->quat1.x());
ASSERT_EQ(testQuat.y(),this->quat1.y());
ASSERT_EQ(testQuat.z(),this->quat1.z());
testQuat = this->quatIdentity*this->quat1;
ASSERT_EQ(testQuat.w(),this->quat1.w());
ASSERT_EQ(testQuat.x(),this->quat1.x());
ASSERT_EQ(testQuat.y(),this->quat1.y());
ASSERT_EQ(testQuat.z(),this->quat1.z());
}
UnitQuaternion&
UnitQuaternion::Divide(
const UnitQuaternion& p,
const UnitQuaternion& q
)
{
UnitQuaternion i;
i.Inverse(q);
Multiply(i, p);
return *this;
}
// Testing of Norm and Normalization for UnitQuaternion
TYPED_TEST (UnitQuaternionsSingleTest, testUnitQuaternionSingleNormalization) {
typedef typename TestFixture::UnitQuaternion UnitQuaternion;
typedef typename TestFixture::UnitQuaternionScalar UnitQuaternionScalar;
typedef typename TestFixture::EigenQuat EigenQuat;
UnitQuaternion testQuat;
UnitQuaternionScalar scalar;
// Check norm and normalization
testQuat = this->quat1;
scalar = testQuat.norm();
ASSERT_NEAR(scalar,1.0,1e-6);
}
// Testing vector() and constructor from vector
TYPED_TEST (UnitQuaternionsSingleTest, testQuaternionSingleVectorAndVectorConstructor) {
typedef typename TestFixture::UnitQuaternion UnitQuaternion;
typedef typename TestFixture::UnitQuaternionScalar UnitQuaternionScalar;
typedef Eigen::Matrix<UnitQuaternionScalar, 4, 1> Vector4;
UnitQuaternion quat = this->quat1;
// vector()
Vector4 vector = quat.vector();
ASSERT_NEAR(quat.w(),vector(0), 1e-3);
ASSERT_NEAR(quat.x(),vector(1), 1e-3);
ASSERT_NEAR(quat.y(),vector(2), 1e-3);
ASSERT_NEAR(quat.z(),vector(3), 1e-3);
// constructor from vector
UnitQuaternion quatFromVector(vector);
ASSERT_NEAR(quat.w(),quatFromVector.w(), 1e-3);
ASSERT_NEAR(quat.x(),quatFromVector.x(), 1e-3);
ASSERT_NEAR(quat.y(),quatFromVector.y(), 1e-3);
ASSERT_NEAR(quat.z(),quatFromVector.z(), 1e-3);
}
// Testing of UnitQuaternion Constructor and Access Operator
TYPED_TEST (UnitQuaternionsSingleTest, testUnitQuaternionSingleConstructor) {
typedef typename TestFixture::UnitQuaternion UnitQuaternion;
typedef typename TestFixture::UnitQuaternionScalar UnitQuaternionScalar;
typedef typename TestFixture::EigenQuat EigenQuat;
// Default constructor of quaternion needs to set all coefficients to zero
UnitQuaternion testQuat;
ASSERT_EQ(testQuat.w(),UnitQuaternionScalar(1));
ASSERT_EQ(testQuat.x(),UnitQuaternionScalar(0));
ASSERT_EQ(testQuat.y(),UnitQuaternionScalar(0));
ASSERT_EQ(testQuat.z(),UnitQuaternionScalar(0));
// Constructor of quaternion using 4 scalars
UnitQuaternion testQuat1(this->eigenQuat1.w(), this->eigenQuat1.x(), this->eigenQuat1.y(), this->eigenQuat1.z());
ASSERT_EQ(testQuat1.w(),this->eigenQuat1.w());
ASSERT_EQ(testQuat1.x(),this->eigenQuat1.x());
ASSERT_EQ(testQuat1.y(),this->eigenQuat1.y());
ASSERT_EQ(testQuat1.z(),this->eigenQuat1.z());
// Constructor of quaternion using real and imaginary part
UnitQuaternion testQuat2(this->eigenQuat1.w(),typename UnitQuaternion::Imaginary(this->eigenQuat1.x(), this->eigenQuat1.y(), this->eigenQuat1.z()));
ASSERT_EQ(testQuat2.w(),this->eigenQuat1.w());
ASSERT_EQ(testQuat2.x(),this->eigenQuat1.x());
ASSERT_EQ(testQuat2.y(),this->eigenQuat1.y());
ASSERT_EQ(testQuat2.z(),this->eigenQuat1.z());
ASSERT_EQ(testQuat2.real(),this->eigenQuat1.w());
ASSERT_EQ(testQuat2.imaginary()(0),this->eigenQuat1.x());
ASSERT_EQ(testQuat2.imaginary()(1),this->eigenQuat1.y());
ASSERT_EQ(testQuat2.imaginary()(2),this->eigenQuat1.z());
// Constructor of quaternion using eigen quaternion
UnitQuaternion testQuat3(this->eigenQuat1);
ASSERT_EQ(testQuat3.w(),this->eigenQuat1.w());
ASSERT_EQ(testQuat3.x(),this->eigenQuat1.x());
ASSERT_EQ(testQuat3.y(),this->eigenQuat1.y());
ASSERT_EQ(testQuat3.z(),this->eigenQuat1.z());
}
UnitQuaternion&
UnitQuaternion::FastLerp(
const UnitQuaternion& p,
const UnitQuaternion& q,
Scalar t
)
{
if (!UseFastLerp)
return Lerp(p,q,t);
Start_Timer(SlerpTime);
Set_Statistic(SlerpCount, SlerpCount+1);
Verify(quaternionFastLerpTableBuilt);
Scalar cosom,sclp,sclq;
cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;
if ( (1.0f + cosom) > 0.01f)
{
// usual case
if ( (1.0f - cosom) > 0.00001f )
{
//usual case
//table_entry = (int)Scaled_Float_To_Bits(cosom, MinCosom, MaxCosom, 10);
float tabled_float = cosom - MinCosom;
int cos_table_entry = Truncate_Float_To_Word(((tabled_float*CosomRangeOverOne) * CosBiggestNumber));
Verify(cos_table_entry >= 0);
Verify(cos_table_entry <= QuaternionLerpTableSize);
#if 0
sclp = Sin((1.0f - t)*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
sclq = Sin(t*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
#else
float difference, percent, lerped_sin;
tabled_float = ((1.0f - t)*Omega_Table[cos_table_entry]) - MinSin;
int sclp_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));
if (!(sclp_table_entry < SinTableSize))
{
Max_Clamp(sclp_table_entry, SinTableSize-1);
}
Verify(sclp_table_entry >= 0 && sclp_table_entry < SinTableSize);
difference = tabled_float - (SinIncrement * sclp_table_entry);
percent = difference / SinIncrement;
int lerp_to_entry = sclp_table_entry + 1;
Max_Clamp(lerp_to_entry, SinTableSize-1);
lerped_sin = Stuff::Lerp(Sin_Table[sclp_table_entry], Sin_Table[lerp_to_entry], percent);
sclp = lerped_sin * SinomOverOne_Table[cos_table_entry];
tabled_float = (t*Omega_Table[cos_table_entry]) - MinSin;
int sclq_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));
Verify(sclq_table_entry >= 0 && sclq_table_entry < SinTableSize);
difference = tabled_float - (SinIncrement * sclq_table_entry);
percent = difference / SinIncrement;
lerp_to_entry = sclq_table_entry + 1;
Max_Clamp(lerp_to_entry, SinTableSize-1);
lerped_sin = Stuff::Lerp(Sin_Table[sclq_table_entry], Sin_Table[lerp_to_entry], percent);
sclq = lerped_sin * SinomOverOne_Table[cos_table_entry];
#endif
}
else
{
// ends very close -- just lerp
sclp = 1.0f - t;
sclq = t;
}
x = sclp*p.x + sclq*q.x;
y = sclp*p.y + sclq*q.y;
z = sclp*p.z + sclq*q.z;
w = sclp*p.w + sclq*q.w;
}
else
{
//SPEW(("jerryeds","SPECIAL CASE"));
/* p and q nearly opposite on sphere-- this is a 360 degree
rotation, but the axis of rotation is undefined, so
slerp really is undefined too. So this apparently picks
an arbitrary plane of rotation. However, I think this
code is incorrect.
*/
//really we want the shortest distance. They are almost on top of each other.
UnitQuaternion r;
r.Subtract(q, p);
Vector3D scaled_rotation;
scaled_rotation = r;
scaled_rotation *= t;
UnitQuaternion scaled_quat;
scaled_quat = scaled_rotation;
Multiply(scaled_quat, p);
Normalize();
}
Stop_Timer(SlerpTime);
return *this;
}
UnitQuaternion &UnitQuaternion::Lerp(const UnitQuaternion& p, const UnitQuaternion& q, Scalar t)
{
Start_Timer(SlerpTime);
Set_Statistic(SlerpCount, SlerpCount+1);
Scalar omega,cosom,sinom,sclp,sclq;
//UnitQuaternion qt;
//UnitQuaternion q = q_temp;
//UnitQuaternion p = p_temp;
cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;
if ( (1.0f + cosom) > 0.01f)
{
// usual case
if ( (1.0f - cosom) > 0.00001f )
{
//usual case
omega = Arccos(cosom);
sinom = Sin(omega);
//SPEW(("jerryeds","omega:%f sinom:%f", omega, sinom));
sclp = Sin((1.0f - t)*omega) / sinom;
sclq = Sin(t*omega) / sinom;
//SPEW(("jerryeds", "* %f %f", sclp, sclq));
}
else
{
// ends very close -- just lerp
sclp = 1.0f - t;
sclq = t;
//SPEW(("jerryeds", "# %f %f", sclp, sclq));
}
x = sclp*p.x + sclq*q.x;
y = sclp*p.y + sclq*q.y;
z = sclp*p.z + sclq*q.z;
w = sclp*p.w + sclq*q.w;
//SPEW(("jerryeds", "r:<%f,%f,%f,%f>",x,y,z,w));
}
else
{
//SPEW(("jerryeds","SPECIAL CASE"));
/* p and q nearly opposite on sphere-- this is a 360 degree
rotation, but the axis of rotation is undefined, so
slerp really is undefined too. So this apparently picks
an arbitrary plane of rotation. However, I think this
code is incorrect.
*/
//really we want the shortest distance. They are almost on top of each other.
UnitQuaternion r;
r.Subtract(q, p);
Vector3D scaled_rotation;
scaled_rotation = r;
scaled_rotation *= t;
UnitQuaternion scaled_quat;
scaled_quat = scaled_rotation;
Multiply(scaled_quat, p);
}
Stop_Timer(SlerpTime);
return *this;
}
Rotation3D::Rotation(const UnitQuaternion& q) {
// quaternion to rotation conversion
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
// http://en.wikipedia.org/wiki/Rotation_group_SO(3)#Quaternions_of_unit_norm
*this << 1 - 2 * q.kY() * q.kY() - 2 * q.kZ() * q.kZ() << 2 * q.kX() * q.kY() - 2 * q.kZ() * q.kW()
<< 2 * q.kX() * q.kZ() + 2 * q.kY() * q.kW() << arma::endr
<< 2 * q.kX() * q.kY() + 2 * q.kZ() * q.kW() << 1 - 2 * q.kX() * q.kX() - 2 * q.kZ() * q.kZ()
<< 2 * q.kY() * q.kZ() - 2 * q.kX() * q.kW() << arma::endr
<< 2 * q.kX() * q.kZ() - 2 * q.kY() * q.kW() << 2 * q.kY() * q.kZ() + 2 * q.kX() * q.kW()
<< 1 - 2 * q.kX() * q.kX() - 2 * q.kY() * q.kY();
}
UnitQuaternion UnitQuaternion::i() const {
UnitQuaternion qi = *this;
// take the congugate, as it is equal to the inverse when a unit vector
qi.imaginary() *= -1;
return qi;
}
UnitQuaternion UnitQuaternion::operator - (const UnitQuaternion& p) const {
return *this * p.i();
}
arma::vec3 UnitQuaternion::rotateVector(const arma::vec3& v) const {
UnitQuaternion vRotated = *this * UnitQuaternion(v) * i();
return vRotated.imaginary();
}
