Math::Quaternion<double> create_quaternion_from_large_real_component(const Math::Matrix4d& matrix)
{
Math::Quaternion<double> result;
result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
result.x() = 0.25 *(matrix(2,1) - matrix(1,2))/result.w();
result.y() = 0.25 *(matrix(0,2) - matrix(2,0))/result.w();
result.z() = 0.25 *(matrix(1,0) - matrix(0,1))/result.w();
return result;
}
TEST_F(QuaternionTest, multiplying_quaternions_with_only_real_part_gives_a_quaternion_with_only_the_real_parts_multiplied_and_no_imaginary_part)
{
Math::Quaternion<double> left;
Math::Quaternion<double> right;
left.w() = create_random_scalar();
right.w() = create_random_scalar();
auto res = left * right;
EXPECT_EQ(left.w() * right.w(), res.w());
EXPECT_EQ(0, res.x());
EXPECT_EQ(0, res.y());
EXPECT_EQ(0, res.z());
}
TEST_F(QuaternionTest, default_quaternion_is_identity_quaternion)
{
const Math::Quaternion<double> quat;
EXPECT_EQ(1, quat.w());
EXPECT_EQ(0, quat.x());
EXPECT_EQ(0, quat.y());
EXPECT_EQ(0, quat.z());
}
void OnlineRotHec::addMeasurement( const Math::Quaternion& q, const Math::Quaternion& r )
{
// make sure the signs of both w's are equal
const double nq = q.w() < 0 ? -1 : 1;
const double nr = r.w() < 0 ? -1 : 1;
Math::ErrorVector< double, 3 > kalmanMeasurement;
kalmanMeasurement.value( 0 ) = r.x() * nr - q.x() * nq;
kalmanMeasurement.value( 1 ) = r.y() * nr - q.y() * nq;
kalmanMeasurement.value( 2 ) = r.z() * nr - q.z() * nq;
kalmanMeasurement.covariance = Math::Matrix< double, 3, 3 >::identity();
// do the filter update
Math::Matrix< double, 3, 3 > h;
skewMatrix( h, Math::Vector< double, 3 >( q.x() * nq + r.x() * nr, q.y() * nq + r.y() * nr, q.z() * nq + r.z() * nr ) );
Tracking::kalmanMeasurementUpdate< 3, 3 >( m_state, Math::Function::LinearFunction< 3, 3, double >( h ),
kalmanMeasurement, 0, m_state.value.size() );
}
Math::Quaternion<double> create_quaternion_from_small_real_component(const Math::Matrix4d& matrix)
{
Math::Quaternion<double> result;
result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
result.x() = 0.5 * std::sqrt(matrix(0,0) - matrix(1,1) - matrix(2,2) + matrix(3,3));
result.y() = 0.5 * std::sqrt(-matrix(0,0) + matrix(1,1) - matrix(2,2) + matrix(3,3));
result.z() = 0.5 * std::sqrt(-matrix(0,0) - matrix(1,1) + matrix(2,2) + matrix(3,3));
return result;
}
Math::Matrix4d make_matrix_from_quaternion(const Math::Quaternion<double>& quat)
{
Math::Matrix4d matrix;
const auto s = 2.0 / quaternion_norm(quat);
matrix(0,0) -= s *(quat.y() * quat.y() + quat.z() * quat.z());
matrix(0,1) += s *(quat.x() * quat.y() - quat.w() * quat.z());
matrix(0,2) += s *(quat.x() * quat.z() + quat.w() * quat.y());
matrix(1,0) += s *(quat.x() * quat.y() + quat.w() * quat.z());
matrix(1,1) -= s *(quat.x() * quat.x() + quat.z() * quat.z());
matrix(1,2) += s *(quat.y() * quat.z() - quat.w() * quat.x());
matrix(2,0) += s *(quat.x() * quat.z() - quat.w() * quat.y());
matrix(2,1) += s *(quat.y() * quat.z() + quat.w() * quat.x());
matrix(2,2) -= s *(quat.x() * quat.x() + quat.y() * quat.y());
return matrix;
}
void EMIHead::lookAt(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix) {
if (!_cost->_emiSkel || !_cost->_emiSkel->_obj)
return;
if (_jointName.empty())
return;
Joint *joint = _cost->_emiSkel->_obj->getJointNamed(_jointName);
if (!joint)
return;
Math::Quaternion lookAtQuat; // Note: Identity if not looking at anything.
if (entering) {
Math::Matrix4 jointToWorld = _cost->getOwner()->getFinalMatrix() * joint->_finalMatrix;
Math::Vector3d jointWorldPos = jointToWorld.getPosition();
Math::Matrix4 worldToJoint = jointToWorld;
worldToJoint.invertAffineOrthonormal();
Math::Vector3d targetDir = (point + _offset) - jointWorldPos;
targetDir.normalize();
const Math::Vector3d worldUp(0, 1, 0);
Math::Vector3d frontDir = Math::Vector3d(worldToJoint(0, 1), worldToJoint(1, 1), worldToJoint(2, 1)); // Look straight ahead. (+Y)
Math::Vector3d modelFront(0, 0, 1);
Math::Vector3d modelUp(0, 1, 0);
joint->_absMatrix.inverseRotate(&modelFront);
joint->_absMatrix.inverseRotate(&modelUp);
// Generate a world-space look at matrix.
Math::Matrix4 lookAtTM;
lookAtTM.setToIdentity();
if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
else
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);
// Convert from world-space to joint-space.
lookAtTM = worldToJoint * lookAtTM;
// Apply angle limits.
Math::Angle p, y, r;
lookAtTM.getXYZ(&y, &p, &r, Math::EO_ZXY);
y.clampDegrees(_yawRange);
p.clampDegrees(_minPitch, _maxPitch);
r.clampDegrees(30.0f);
lookAtTM.buildFromXYZ(y, p, r, Math::EO_ZXY);
lookAtQuat.fromMatrix(lookAtTM.getRotation());
}
if (_headRot != lookAtQuat) {
Math::Quaternion diff = _headRot.inverse() * lookAtQuat;
float angle = 2 * acos(diff.w());
if (diff.w() < 0.0f) {
angle = 2 * (float)M_PI - angle;
}
float turnAmount = g_grim->getPerSecond(rate * ((float)M_PI / 180.0f));
if (turnAmount < angle)
_headRot = _headRot.slerpQuat(lookAtQuat, turnAmount / angle);
else
_headRot = lookAtQuat;
}
if (_headRot != Math::Quaternion()) { // If not identity..
joint->_animMatrix = joint->_animMatrix * _headRot.toMatrix();
joint->_animQuat = joint->_animQuat * _headRot;
_cost->_emiSkel->_obj->commitAnim();
}
}
