const bool Test_Matrix_RotationMatrix()
{
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TVector4d kTransformedA_d = VectorMultiply(TVector4d(), kOrientation_d, TVector4d{1.0, 0.0, 0.0, 1.0});
const TVector3d kTransformedB_d{kTransformedA_d.m_dX, kTransformedA_d.m_dY, kTransformedA_d.m_dZ};
const TVector3d kTransformedC_d = QuaternionRotate(TVector3d(), TVector3d{1.0, 0.0, 0.0}, kQuatOrient_d);
const bool kbPass_d = Equal(kTransformedB_d, kTransformedC_d, s_kdEpsilon);
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TVector4f kTransformedA_f = VectorMultiply(TVector4f(), kOrientation_f, TVector4f{1.0f, 0.0f, 0.0f, 1.0f});
const TVector3f kTransformedB_f{kTransformedA_f.m_fX, kTransformedA_f.m_fY, kTransformedA_f.m_fZ};
const TVector3f kTransformedC_f = QuaternionRotate(TVector3f(), TVector3f{1.0f, 0.0f, 0.0f}, kQuatOrient_f);
const bool kbPass_f = Equal(kTransformedB_f, kTransformedC_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
void MotionEmuThread() {
auto update_time = std::chrono::steady_clock::now();
Math::Quaternion<float> q = MakeQuaternion(Math::Vec3<float>(), 0);
Math::Quaternion<float> old_q;
while (!shutdown_event.WaitUntil(update_time)) {
update_time += update_duration;
old_q = q;
{
std::lock_guard<std::mutex> guard(tilt_mutex);
// Find the quaternion describing current 3DS tilting
q = MakeQuaternion(Math::MakeVec(-tilt_direction.y, 0.0f, tilt_direction.x),
tilt_angle);
}
auto inv_q = q.Inverse();
// Set the gravity vector in world space
auto gravity = Math::MakeVec(0.0f, -1.0f, 0.0f);
// Find the angular rate vector in world space
auto angular_rate = ((q - old_q) * inv_q).xyz * 2;
angular_rate *= 1000 / update_millisecond / MathUtil::PI * 180;
// Transform the two vectors from world space to 3DS space
gravity = QuaternionRotate(inv_q, gravity);
angular_rate = QuaternionRotate(inv_q, angular_rate);
// Update the sensor state
{
std::lock_guard<std::mutex> guard(status_mutex);
status = std::make_tuple(gravity, angular_rate);
}
}
}
void RigidBody::UpdatePosition()
{
if (m_IsDynamic)
{
EditOwner()->EditChild<Transform>()->EditPosition() += m_Velocity* Constants::PhysicsDeltaTime;
Quaternion RotQuat(1, m_AngularVelocity[0] * Constants::PhysicsDeltaTime * 0.5f, m_AngularVelocity[1] * Constants::PhysicsDeltaTime * 0.5f, m_AngularVelocity[2] * Constants::PhysicsDeltaTime * 0.5f);
QuaternionRotate(GetOwner()->GetChild<Transform>()->GetRotation(), m_AngularVelocity, m_AngularMomentum);
RotQuat.normalize();
if (!RotQuat.isZero())
{
EditOwner()->EditChild<Transform>()->EditRotation() *= RotQuat;
EditOwner()->EditChild<Transform>()->EditRotation().normalize();
}
else
{
m_AngularMomentum = Vector3::Zero;
}
}
}
const bool Test_Matrix_TransformationMatrix()
{
const TVector3d kTranslate_d{1.0, 2.0, 3.0};
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const double kdScale = 2.0;
const TMatrix4d kTranslation_d = TranslationMatrix(TMatrix4d(), kTranslate_d);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TMatrix4d kScaling_d = ScalingMatrix(TMatrix4d(), kdScale, kdScale, kdScale);
const TMatrix4d kTransformA_d = Multiply(TMatrix4d(), kScaling_d, kOrientation_d);
const TMatrix4d kTransformB_d = Multiply(TMatrix4d(), kTranslation_d, kTransformA_d);
const TVector3d kBasisX_d = QuaternionRotate(TVector3d(), TVector3d{kdScale, 0.0, 0.0}, kQuatOrient_d);
const TVector3d kBasisY_d = QuaternionRotate(TVector3d(), TVector3d{0.0, kdScale, 0.0}, kQuatOrient_d);
const TVector3d kBasisZ_d = QuaternionRotate(TVector3d(), TVector3d{0.0, 0.0, kdScale}, kQuatOrient_d);
const TMatrix4d kTransformC_d = TransformationMatrix(TMatrix4d(), kBasisX_d, kBasisY_d, kBasisZ_d, kTranslate_d);
const bool kbPass_d = Equal(kTransformC_d, kTransformB_d, s_kdEpsilon);
//
const TVector3f kTranslate_f{1.0f, 2.0f, 3.0f};
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const float kfScale = 2.0f;
const TMatrix4f kTranslation_f = TranslationMatrix(TMatrix4f(), kTranslate_f);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TMatrix4f kScaling_f = ScalingMatrix(TMatrix4f(), kfScale, kfScale, kfScale);
const TMatrix4f kTransformA_f = Multiply(TMatrix4f(), kScaling_f, kOrientation_f);
const TMatrix4f kTransformB_f = Multiply(TMatrix4f(), kTranslation_f, kTransformA_f);
const TVector3f kBasisX_f = QuaternionRotate(TVector3f(), TVector3f{kfScale, 0.0f, 0.0f}, kQuatOrient_f);
const TVector3f kBasisY_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, kfScale, 0.0f}, kQuatOrient_f);
const TVector3f kBasisZ_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, 0.0f, kfScale}, kQuatOrient_f);
const TMatrix4f kTransformC_f = TransformationMatrix(TMatrix4f(), kBasisX_f, kBasisY_f, kBasisZ_f, kTranslate_f);
const bool kbPass_f = Equal(kTransformC_f, kTransformB_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
void QuaternionOperations::testQuaternionRotate()
{
COORD_3D coordIn, coordTarget, coordOut;
QUATERNION q;
q.q0 = 9.999955e-01f;
q.q1 = -2.908411e-03f;
q.q2 = 7.327030e-04;
q.q3 = 1.942619e-06;
coordTarget.x = -1.485461e-02f;
coordTarget.y = -5.946447e-02f;
coordTarget.z = 1.004467e+00f;
coordIn.x = -1.632653e-02f;
coordIn.y = -6.530612e-02f;
coordIn.z = 1.004082e+00f;
CPPUNIT_ASSERT(QuaternionRotate(&coordIn, &q, &coordOut));
CPPUNIT_ASSERT_DOUBLES_EQUAL(coordTarget.x, coordOut.x, 1e-5f);
CPPUNIT_ASSERT_DOUBLES_EQUAL(coordTarget.y, coordOut.y, 1e-5f);
CPPUNIT_ASSERT_DOUBLES_EQUAL(coordTarget.z, coordOut.z, 1e-5f);
}
