From e63caf5a564788a882d736bb10ad5e20af1b7957 Mon Sep 17 00:00:00 2001
From: Sebastian Eibl <sebastian.eibl@fau.de>
Date: Tue, 6 Nov 2018 15:25:20 +0100
Subject: [PATCH] replaced the common R*A*R^T pattern with an optimized version
---
src/core/math/Matrix3.h | 20 ++++++++++++++++++--
src/pe/rigidbody/RigidBody.h | 8 ++++----
tests/core/math/Matrix3Test.cpp | 8 ++++++++
3 files changed, 30 insertions(+), 6 deletions(-)
diff --git a/src/core/math/Matrix3.h b/src/core/math/Matrix3.h
index 3f38c096b..54119bee9 100644
--- a/src/core/math/Matrix3.h
+++ b/src/core/math/Matrix3.h
@@ -1677,8 +1677,24 @@ Matrix3< typename MathTrait<T0,T1>::High > tensorProduct( Vector3<T0> v0, Vector
}
//**********************************************************************************************************************
-
-
+/**
+ * Equivalent to R*A*R.getTranspose().
+ */
+template< typename Type>
+inline Matrix3< Type > transformMatrixRART( const Matrix3<Type>& R, const Matrix3<Type>& A )
+{
+ const auto r0 = A[0]*R[0]*R[0] + A[1]*R[0]*R[1] + A[2]*R[0]*R[2] + A[3]*R[0]*R[1] + A[4]*R[1]*R[1] + A[5]*R[1]*R[2] + A[6]*R[0]*R[2] + A[7]*R[1]*R[2] + A[8]*R[2]*R[2];
+ const auto r1 = A[0]*R[0]*R[3] + A[1]*R[0]*R[4] + A[2]*R[0]*R[5] + A[3]*R[1]*R[3] + A[4]*R[1]*R[4] + A[5]*R[1]*R[5] + A[6]*R[2]*R[3] + A[7]*R[2]*R[4] + A[8]*R[2]*R[5];
+ const auto r2 = A[0]*R[0]*R[6] + A[1]*R[0]*R[7] + A[2]*R[0]*R[8] + A[3]*R[1]*R[6] + A[4]*R[1]*R[7] + A[5]*R[1]*R[8] + A[6]*R[2]*R[6] + A[7]*R[2]*R[7] + A[8]*R[2]*R[8];
+ const auto r3 = A[0]*R[0]*R[3] + A[1]*R[1]*R[3] + A[2]*R[2]*R[3] + A[3]*R[0]*R[4] + A[4]*R[1]*R[4] + A[5]*R[2]*R[4] + A[6]*R[0]*R[5] + A[7]*R[1]*R[5] + A[8]*R[2]*R[5];
+ const auto r4 = A[0]*R[3]*R[3] + A[1]*R[3]*R[4] + A[2]*R[3]*R[5] + A[3]*R[3]*R[4] + A[4]*R[4]*R[4] + A[5]*R[4]*R[5] + A[6]*R[3]*R[5] + A[7]*R[4]*R[5] + A[8]*R[5]*R[5];
+ const auto r5 = A[0]*R[3]*R[6] + A[1]*R[3]*R[7] + A[2]*R[3]*R[8] + A[3]*R[4]*R[6] + A[4]*R[4]*R[7] + A[5]*R[4]*R[8] + A[6]*R[5]*R[6] + A[7]*R[5]*R[7] + A[8]*R[5]*R[8];
+ const auto r6 = A[0]*R[0]*R[6] + A[1]*R[1]*R[6] + A[2]*R[2]*R[6] + A[3]*R[0]*R[7] + A[4]*R[1]*R[7] + A[5]*R[2]*R[7] + A[6]*R[0]*R[8] + A[7]*R[1]*R[8] + A[8]*R[2]*R[8];
+ const auto r7 = A[0]*R[3]*R[6] + A[1]*R[4]*R[6] + A[2]*R[5]*R[6] + A[3]*R[3]*R[7] + A[4]*R[4]*R[7] + A[5]*R[5]*R[7] + A[6]*R[3]*R[8] + A[7]*R[4]*R[8] + A[8]*R[5]*R[8];
+ const auto r8 = A[0]*R[6]*R[6] + A[1]*R[6]*R[7] + A[2]*R[6]*R[8] + A[3]*R[6]*R[7] + A[4]*R[7]*R[7] + A[5]*R[7]*R[8] + A[6]*R[6]*R[8] + A[7]*R[7]*R[8] + A[8]*R[8]*R[8];
+
+ return Matrix3<Type>(r0,r1,r2,r3,r4,r5,r6,r7,r8);
+}
} // namespace math
diff --git a/src/pe/rigidbody/RigidBody.h b/src/pe/rigidbody/RigidBody.h
index fdebc3ff6..4f872a685 100644
--- a/src/pe/rigidbody/RigidBody.h
+++ b/src/pe/rigidbody/RigidBody.h
@@ -874,7 +874,7 @@ inline const Mat3& RigidBody::getBodyInertia() const
*/
inline const Mat3 RigidBody::getInertia() const
{
- return R_ * I_ * R_.getTranspose();
+ return math::transformMatrixRART(R_, I_);
}
//*************************************************************************************************
@@ -898,7 +898,7 @@ inline const Mat3& RigidBody::getInvBodyInertia() const
*/
inline const Mat3 RigidBody::getInvInertia() const
{
- return R_ * Iinv_ * R_.getTranspose();
+ return math::transformMatrixRART(R_, Iinv_);
}
//*************************************************************************************************
@@ -1810,7 +1810,7 @@ inline void RigidBody::addImpulseAtPos( const Vec3& j, const Vec3& p )
{
if( !hasSuperBody() ) {
v_ += j * invMass_;
- w_ += ( R_ * Iinv_ * R_.getTranspose() ) * ( ( p - gpos_ ) % j );
+ w_ += getInvInertia() * ( ( p - gpos_ ) % j );
wake();
}
else sb_->addImpulseAtPos( j, p );
@@ -2002,7 +2002,7 @@ inline const Vec3 RigidBody::accFromWF( const Vec3& gpos ) const
const Vec3 vdot( force_ * invMass_ );
// Calculating the angular acceleration
- const Vec3 wdot( R_ * Iinv_ * R_.getTranspose() * ( torque_ - w_ % ( R_ * I_ * R_.getTranspose() * w_ ) ) );
+ const Vec3 wdot( getInvInertia() * ( torque_ - w_ % ( getInertia() * w_ ) ) );
// Calculating the distance to the center of mass of the superordinate body
const Vec3 r( gpos - gpos_ );
diff --git a/tests/core/math/Matrix3Test.cpp b/tests/core/math/Matrix3Test.cpp
index 1cfbb83a2..7fd6aea27 100644
--- a/tests/core/math/Matrix3Test.cpp
+++ b/tests/core/math/Matrix3Test.cpp
@@ -62,6 +62,13 @@ void rotationTest()
WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( result, cmp, real_t(1e-5) );
}
+void RARTTest()
+{
+ Matrix3<real_t> A ( 1,2,3,4,5,6,7,8,9 );
+ Matrix3<real_t> R ( 2,3,4,5,6,7,8,9,1 );
+ WALBERLA_CHECK_FLOAT_EQUAL( math::transformMatrixRART(R,A), R * A * R.getTranspose() );
+}
+
int main()
{
@@ -72,6 +79,7 @@ int main()
m1 * m2;
rotationTest();
+ RARTTest();
return 0;
}
--
GitLab