diff --git a/src/core/math/Matrix3.h b/src/core/math/Matrix3.h index 31f129e7db42db8e9e1c2249f265fec8f2d4d105..8706833ae539070102d9258ece6c8faecf1016e1 100644 --- a/src/core/math/Matrix3.h +++ b/src/core/math/Matrix3.h @@ -1693,6 +1693,25 @@ inline const Matrix3 fabs( const Matrix3& m ) //********************************************************************************************************************** +//********************************************************************************************************************** +/*!\fn bool math::isinf( const Matrix3& m ) +// \brief Checks the given matrix for infinite elements. +// +// \param m The matrix to be checked for infinite elements. +// \return \a true if at least one element of the matrix is infinite, \a false otherwise. +*/ +template< typename Type > +inline bool isinf( const Matrix3& m ) +{ + if( math::isinf( m[0] ) || math::isinf( m[1] ) || math::isinf( m[2] ) || + math::isinf( m[3] ) || math::isinf( m[4] ) || math::isinf( m[5] ) || + math::isinf( m[6] ) || math::isinf( m[7] ) || math::isinf( m[8] ) ) + return true; + else return false; +} +//********************************************************************************************************************** + + //********************************************************************************************************************** // \brief Calculates the tensor product of two vectors. //********************************************************************************************************************** diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp b/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp index a41604b65475396c36708b3dfaa03ff6f5659086..aa6eb1c8a862199bb9e5d9a9321d437cb6cfcfe1 100644 --- a/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp +++ b/src/mesh/pe/rigid_body/ConvexPolyhedron.cpp @@ -90,14 +90,15 @@ void ConvexPolyhedron::init( const Vec3& gpos, const Vec3& rpos, const Quat& q, q_ = q; // Setting the orientation R_ = q_.toRotationMatrix(); // Setting the rotation matrix - // Calculating the ConvexPolyhedron mass - mass_ = getVolume() * Material::getDensity( getMaterial() ); - - // Calculating the moment of inertia - calcInertia(); - setGlobal( global ); - setMass( infiniteMass ); // sets inverse mass and interatio tensor + if (infiniteMass) + { + setMassAndInertiaToInfinity(); + } else + { + // sets inverse mass and interatio tensor + setMassAndInertia( getVolume() * Material::getDensity( getMaterial() ), mesh::computeIntertiaTensor( mesh_ ) ); + } setCommunicating( communicating ); setFinite( true ); @@ -159,17 +160,6 @@ void ConvexPolyhedron::calcBoundingBox() } //************************************************************************************************* -//************************************************************************************************* -/*!\brief Calculation of the moment of inertia in reference to the body frame of the ConvexPolyhedron. - * - * \return void - */ -void ConvexPolyhedron::calcInertia() -{ - I_ = mesh::computeIntertiaTensor( mesh_ ); -} -//************************************************************************************************* - //************************************************************************************************* /*!\brief Calculation of the volume of the ConvexPolyhedron. diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedron.h b/src/mesh/pe/rigid_body/ConvexPolyhedron.h index 33ff28ac9fcaf613a196d7fc90fca7f6f7499f21..46e95afbda14c17c679ec2f34e98b8041387b8fc 100644 --- a/src/mesh/pe/rigid_body/ConvexPolyhedron.h +++ b/src/mesh/pe/rigid_body/ConvexPolyhedron.h @@ -127,7 +127,6 @@ protected: /*!\name Utility functions */ //@{ virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box - void calcInertia(); // Calculation of the moment of inertia virtual TriangleMesh::VertexHandle supportVertex( const TriangleMesh::Normal & d, const TriangleMesh::VertexHandle startVertex ) const; //@} //********************************************************************************************** diff --git a/src/pe/cr/HCSITS.impl.h b/src/pe/cr/HCSITS.impl.h index 45ed1e1ddd7f22c5a3f69ee9a5dbeb6a6bf273d8..a67882dc306140900bc9c02cb840fce0019b5ae9 100644 --- a/src/pe/cr/HCSITS.impl.h +++ b/src/pe/cr/HCSITS.impl.h @@ -288,7 +288,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d for( auto body = globalBodyStorage_->begin(); body != globalBodyStorage_->end(); ++body, ++j ) { body->wake(); // BUGFIX: Force awaking of all bodies! body->index_ = j; - WALBERLA_ASSERT( body->hasInfiniteMass(), "fatal" ); + WALBERLA_CHECK( body->hasInfiniteMass(), "Global bodies need to have infinite mass as they are not communicated!" ); initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity @@ -347,7 +347,7 @@ inline void HardContactSemiImplicitTimesteppingSolvers::timestep( const real_t d for( auto body = globalBodyStorage_->begin(); body != globalBodyStorage_->end(); ++body, ++j ) { body->wake(); // BUGFIX: Force awaking of all bodies! body->index_ = j; - WALBERLA_ASSERT( body->hasInfiniteMass(), "fatal" ); + WALBERLA_CHECK( body->hasInfiniteMass(), "Global bodies need to have infinite mass as they are not communicated!" ); initializeVelocityCorrections( *body, bodyCache.dv_[j], bodyCache.dw_[j], dt ); // use applied external forces to calculate starting velocity diff --git a/src/pe/rigidbody/Box.cpp b/src/pe/rigidbody/Box.cpp index 417af0f24293989aee22a9f84499cde30f8d8a6e..ffd27fc8ebc5639cf5f6ad794c3e45c22fee8a5a 100644 --- a/src/pe/rigidbody/Box.cpp +++ b/src/pe/rigidbody/Box.cpp @@ -75,15 +75,15 @@ Box::Box( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q, q_ = q; // Setting the orientation R_ = q_.toRotationMatrix(); // Setting the rotation matrix - // Calculating the sphere mass - mass_ = calcMass( lengths, Material::getDensity( material ) ); - invMass_ = real_c(1) / mass_; - - // Calculating the moment of inertia - calcInertia(); - setGlobal( global ); - setMass( infiniteMass ); + if (infiniteMass) + { + setMassAndInertiaToInfinity(); + } else + { + auto mass = calcMass( lengths, Material::getDensity( material ) ); + setMassAndInertia( mass, calcInertia( lengths, mass ) ); + } setCommunicating( communicating ); setFinite( true ); @@ -130,8 +130,8 @@ Box::~Box() bool Box::containsRelPointImpl( real_t px, real_t py, real_t pz ) const { return std::fabs(px) <= real_t(0.5)*lengths_[0] && - std::fabs(py) <= real_t(0.5)*lengths_[1] && - std::fabs(pz) <= real_t(0.5)*lengths_[2]; + std::fabs(py) <= real_t(0.5)*lengths_[1] && + std::fabs(pz) <= real_t(0.5)*lengths_[2]; } //************************************************************************************************* @@ -307,7 +307,7 @@ void Box::calcBoundingBox() const real_t ylength( real_t(0.5) * ( fabs(R_[3]*lengths_[0]) + fabs(R_[4]*lengths_[1]) + fabs(R_[5]*lengths_[2]) ) + contactThreshold ); const real_t zlength( real_t(0.5) * ( fabs(R_[6]*lengths_[0]) + fabs(R_[7]*lengths_[1]) + fabs(R_[8]*lengths_[2]) ) + contactThreshold ); aabb_ = math::AABB( - gpos_[0] - xlength, + gpos_[0] - xlength, gpos_[1] - ylength, gpos_[2] - zlength, gpos_[0] + xlength, @@ -326,12 +326,12 @@ void Box::calcBoundingBox() * * \return void */ -void Box::calcInertia() +Mat3 Box::calcInertia(const Vec3& length, const real_t mass) { - I_[0] = mass_/static_cast( 12 ) * ( lengths_[1]*lengths_[1] + lengths_[2]*lengths_[2] ); - I_[4] = mass_/static_cast( 12 ) * ( lengths_[0]*lengths_[0] + lengths_[2]*lengths_[2] ); - I_[8] = mass_/static_cast( 12 ) * ( lengths_[0]*lengths_[0] + lengths_[1]*lengths_[1] ); - Iinv_ = I_.getInverse(); + return Mat3::makeDiagonalMatrix( + mass/static_cast( 12 ) * ( length[1]*length[1] + length[2]*length[2] ), + mass/static_cast( 12 ) * ( length[0]*length[0] + length[2]*length[2] ), + mass/static_cast( 12 ) * ( length[0]*length[0] + length[1]*length[1] )); } //************************************************************************************************* diff --git a/src/pe/rigidbody/Box.h b/src/pe/rigidbody/Box.h index b76ab998319134bdb84faacaade1c192ea4543e0..cd6ab866230a6fc09d0bbb14b49eed97a34e43c3 100644 --- a/src/pe/rigidbody/Box.h +++ b/src/pe/rigidbody/Box.h @@ -96,6 +96,7 @@ public: static inline real_t calcMass( const Vec3& l, real_t density ); static inline real_t calcDensity( real_t x, real_t y, real_t z, real_t mass ); static inline real_t calcDensity( const Vec3& l, real_t mass ); + static inline Mat3 calcInertia( const Vec3& length, const real_t mass); // Calculation of the moment of inertia //@} //********************************************************************************************** @@ -143,7 +144,6 @@ protected: /*!\name Utility functions */ //@{ virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box - void calcInertia(); // Calculation of the moment of inertia //@} //********************************************************************************************** diff --git a/src/pe/rigidbody/Capsule.cpp b/src/pe/rigidbody/Capsule.cpp index 0a99487ef92f44a3b7b25134765727dfc994d501..7b58b0ce64b744bd31657b8cd63e93863fc9603d 100644 --- a/src/pe/rigidbody/Capsule.cpp +++ b/src/pe/rigidbody/Capsule.cpp @@ -75,15 +75,15 @@ Capsule::Capsule( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const q_ = q; // Setting the orientation R_ = q_.toRotationMatrix(); // Setting the rotation matrix - // Calculating the capsule mass - mass_ = radius*radius * math::M_PI * ( real_t(4)/real_t(3) * radius + length ) * Material::getDensity( material ); - invMass_ = real_t(1) / mass_; - - // Calculating the moment of inertia - calcInertia(); - setGlobal( global ); - setMass( infiniteMass ); + if (infiniteMass) + { + setMassAndInertiaToInfinity(); + } else + { + auto mass = calcMass( radius, length, Material::getDensity( material ) ); + setMassAndInertia( mass, calcInertia( radius, length, Material::getDensity( material ) ) ); + } setCommunicating( communicating ); setFinite( true ); @@ -192,29 +192,25 @@ void Capsule::calcBoundingBox() * * \return void */ -void Capsule::calcInertia() +Mat3 Capsule::calcInertia( const real_t radius, const real_t length, const real_t density) { - const real_t density( calcDensity( radius_, length_, mass_ ) ); - const real_t sphereMass( real_t (4)/real_t (3) * math::M_PI * radius_*radius_*radius_ * density ); - const real_t cylinderMass( math::M_PI * radius_*radius_ * length_ * density ); + const real_t sphereMass( real_t (4)/real_t (3) * math::M_PI * radius*radius*radius * density ); + const real_t cylinderMass( math::M_PI * radius*radius * length * density ); // 'Ia' represent the moment of inertia along the x-axis. 'Ia' contains the following two parts: // - cylinder : I = (1/2)*mass*radius^2 // - sphere : I = (2/5)*mass*radius^2 - const real_t Ia( radius_*radius_ * ( real_t (0.5)*cylinderMass + real_t (0.4)*sphereMass ) ); + const real_t Ia( radius*radius * ( real_t (0.5)*cylinderMass + real_t (0.4)*sphereMass ) ); // 'Ib' represent the moment of inertia along the y- and z-axis. 'Ib' contains the following two parts, // where full_length is the full length of the cylinder part and half_length is (1/2)*full_length: // - cylinder : I = mass*( (1/4)*radius^2 + (1/12)*full_length^2 ) // - sphere : I = mass*( (2/5)*radius^2 + half_length^2 + (3/4)*half_length*radius ) - const real_t Ib( cylinderMass*( real_t (0.25)*radius_*radius_ + real_t (1)/real_t (12)*length_*length_ ) + - sphereMass*( real_t (0.4)*radius_*radius_ + real_t (0.375)*radius_*length_ + real_t (0.25)*length_*length_ ) ); + const real_t Ib( cylinderMass*( real_t (0.25)*radius*radius + real_t (1)/real_t (12)*length*length ) + + sphereMass*( real_t (0.4)*radius*radius + real_t (0.375)*radius*length + real_t (0.25)*length*length ) ); // Setting the moment of inertia (capsule is aligned along the x-axis) - I_[0] = Ia; - I_[4] = Ib; - I_[8] = Ib; - Iinv_ = I_.getInverse(); + return Mat3::makeDiagonalMatrix(Ia, Ib, Ib); } //************************************************************************************************* diff --git a/src/pe/rigidbody/Capsule.h b/src/pe/rigidbody/Capsule.h index 3be8e04530f9b725e2831de746ba8c1875143b98..55070d0224f835f8870f1be99e3eb432f936a6e0 100644 --- a/src/pe/rigidbody/Capsule.h +++ b/src/pe/rigidbody/Capsule.h @@ -106,6 +106,7 @@ public: static inline real_t calcVolume( real_t radius, real_t length ); static inline real_t calcMass( real_t radius, real_t length, real_t density ); static inline real_t calcDensity( real_t radius, real_t length, real_t mass ); + static Mat3 calcInertia( const real_t radius, const real_t length, const real_t density); //@} //********************************************************************************************** @@ -139,7 +140,6 @@ protected: /*!\name Utility functions */ //@{ virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box - void calcInertia(); // Calculation of the moment of inertia //@} //********************************************************************************************** diff --git a/src/pe/rigidbody/CylindricalBoundary.cpp b/src/pe/rigidbody/CylindricalBoundary.cpp index 1a96385517c916ce10aafe20132d840269aa5178..6b99c946384f11f2ecc5cd9f3d88a4e65956622d 100644 --- a/src/pe/rigidbody/CylindricalBoundary.cpp +++ b/src/pe/rigidbody/CylindricalBoundary.cpp @@ -61,7 +61,7 @@ CylindricalBoundary::CylindricalBoundary( id_t sid, id_t uid, const Vec3& gpos, //boundaries are always considered locally and have infinite mass setGlobal( true ); setCommunicating( false ); - setMass( true ); + setMassAndInertiaToInfinity(); setFinite( false ); // Checking the radius diff --git a/src/pe/rigidbody/Ellipsoid.cpp b/src/pe/rigidbody/Ellipsoid.cpp index c3a0b6bb8c4c48b745b1f7db34511d96fb7af0eb..3c68d587590468955490f1b60f362f3fbe882007 100644 --- a/src/pe/rigidbody/Ellipsoid.cpp +++ b/src/pe/rigidbody/Ellipsoid.cpp @@ -84,15 +84,15 @@ Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, c q_ = q; // Setting the orientation R_ = q_.toRotationMatrix(); // Setting the rotation matrix - // Calculating the Ellipsoid mass - mass_ = calcMass(semiAxes_, Material::getDensity( material )); - invMass_ = real_c(1) / mass_; - - // Calculating the moment of inertia - calcInertia(); - setGlobal( global ); - setMass( infiniteMass ); + if (infiniteMass) + { + setMassAndInertiaToInfinity(); + } else + { + auto mass = calcMass( semiAxes, Material::getDensity( material ) ); + setMassAndInertia( mass, calcInertia( mass, semiAxes ) ); + } setCommunicating( communicating ); setFinite( true ); diff --git a/src/pe/rigidbody/Ellipsoid.h b/src/pe/rigidbody/Ellipsoid.h index f2baad173f8f64029055721511af7c220a06e400..e723bc1b9e5cdbe70530b500c2a7459381e1ddd9 100644 --- a/src/pe/rigidbody/Ellipsoid.h +++ b/src/pe/rigidbody/Ellipsoid.h @@ -55,8 +55,8 @@ namespace pe { * \brief Base class for the Ellipsoid geometry. * * The Ellipsoid class represents the base class for the Ellipsoid geometry. It provides - * the basic functionality of a Ellipsoid. An Ellipsoid is obtained from a sphere by deforming it by means - * of directional scalings. Its three semi-axes corresponding to the x, y, z direction are labeled + * the basic functionality of a Ellipsoid. An Ellipsoid is obtained from a sphere by deforming it by means + * of directional scalings. Its three semi-axes corresponding to the x, y, z direction are labeled * a, b, c. */ class Ellipsoid : public GeomPrimitive @@ -66,11 +66,11 @@ public: /*!\name Constructors */ //@{ explicit Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q, - const Vec3& semiAxes, MaterialID material, - const bool global, const bool communicating, const bool infiniteMass ); + const Vec3& semiAxes, MaterialID material, + const bool global, const bool communicating, const bool infiniteMass ); explicit Ellipsoid( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q, - const Vec3& semiAxes, MaterialID material, - const bool global, const bool communicating, const bool infiniteMass ); + const Vec3& semiAxes, MaterialID material, + const bool global, const bool communicating, const bool infiniteMass ); //@} //********************************************************************************************** @@ -116,6 +116,7 @@ public: static inline real_t calcVolume( const Vec3& semiAxes ); static inline real_t calcMass( const Vec3& semiAxes, real_t density ); static inline real_t calcDensity( const Vec3& semiAxes, real_t mass ); + static inline Mat3 calcInertia( const real_t mass, const Vec3& semiAxes ); //@} //********************************************************************************************** @@ -132,7 +133,6 @@ protected: /*!\name Utility functions */ //@{ inline virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box - inline void calcInertia(); // Calculation of the moment of inertia //@} //********************************************************************************************** @@ -140,7 +140,7 @@ protected: /*!\name Member variables */ //@{ Vec3 semiAxes_; //!< Semi-axes of the Ellipsoid. - /*!< The radius is constrained to values larger than 0.0. */ + /*!< The radius is constrained to values larger than 0.0. */ //@} //********************************************************************************************** private: @@ -264,14 +264,14 @@ inline void Ellipsoid::calcBoundingBox() const real_t ylength( fabs(R_[3]*semiAxes_[0]) + fabs(R_[4]*semiAxes_[1]) + fabs(R_[5]*semiAxes_[2]) + contactThreshold ); const real_t zlength( fabs(R_[6]*semiAxes_[0]) + fabs(R_[7]*semiAxes_[1]) + fabs(R_[8]*semiAxes_[2]) + contactThreshold ); aabb_ = math::AABB( - gpos_[0] - xlength, + gpos_[0] - xlength, gpos_[1] - ylength, gpos_[2] - zlength, gpos_[0] + xlength, gpos_[1] + ylength, gpos_[2] + zlength ); - // WALBERLA_ASSERT( aabb_.isValid() , "Invalid bounding box detected" ); + // WALBERLA_ASSERT( aabb_.isValid() , "Invalid bounding box detected" ); WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos_ << "\nlengths: " << xlength << "," << ylength << ", " << zlength<< "\nvel: " << getLinearVel() << "\nbox: " << aabb_ ); } //************************************************************************************************* @@ -282,12 +282,12 @@ inline void Ellipsoid::calcBoundingBox() * * \return void */ -inline void Ellipsoid::calcInertia() +inline Mat3 Ellipsoid::calcInertia( const real_t mass, const Vec3& semiAxes ) { - I_[0] = real_c(0.2) * mass_ * (semiAxes_[1] * semiAxes_[1] + semiAxes_[2] * semiAxes_[2]); - I_[4] = real_c(0.2) * mass_ * (semiAxes_[2] * semiAxes_[2] + semiAxes_[0] * semiAxes_[0]); - I_[8] = real_c(0.2) * mass_ * (semiAxes_[0] * semiAxes_[0] + semiAxes_[1] * semiAxes_[1]); - Iinv_ = I_.getInverse(); + return Mat3::makeDiagonalMatrix( + real_c(0.2) * mass * (semiAxes[1] * semiAxes[1] + semiAxes[2] * semiAxes[2]), + real_c(0.2) * mass * (semiAxes[2] * semiAxes[2] + semiAxes[0] * semiAxes[0]), + real_c(0.2) * mass * (semiAxes[0] * semiAxes[0] + semiAxes[1] * semiAxes[1])); } //************************************************************************************************* @@ -303,11 +303,11 @@ inline Vec3 Ellipsoid::support( const Vec3& d ) const auto len = d.sqrLength(); if (!math::equal(len, real_t(0))) { - Vec3 d_loc = vectorFromWFtoBF(d); - Vec3 norm_vec(d_loc[0] * semiAxes_[0], d_loc[1] * semiAxes_[1], d_loc[2] * semiAxes_[2]); - real_t norm_length = norm_vec.length(); - Vec3 local_support = (real_t(1.0) / norm_length) * Vec3(semiAxes_[0] * semiAxes_[0] * d_loc[0], - semiAxes_[1] * semiAxes_[1] * d_loc[1], semiAxes_[2] * semiAxes_[2] * d_loc[2]); + Vec3 d_loc = vectorFromWFtoBF(d); + Vec3 norm_vec(d_loc[0] * semiAxes_[0], d_loc[1] * semiAxes_[1], d_loc[2] * semiAxes_[2]); + real_t norm_length = norm_vec.length(); + Vec3 local_support = (real_t(1.0) / norm_length) * Vec3(semiAxes_[0] * semiAxes_[0] * d_loc[0], + semiAxes_[1] * semiAxes_[1] * d_loc[1], semiAxes_[2] * semiAxes_[2] * d_loc[2]); return pointFromBFtoWF(local_support); } else { diff --git a/src/pe/rigidbody/Plane.cpp b/src/pe/rigidbody/Plane.cpp index ffb6c91a0690eaabb52bb7c45199b0255506deb3..872edbe3e2a7a6ab3324204ae1e12ee4984878b8 100644 --- a/src/pe/rigidbody/Plane.cpp +++ b/src/pe/rigidbody/Plane.cpp @@ -71,14 +71,14 @@ Plane::Plane( id_t sid, id_t uid, //planes are always considered locally and have infinite mass setGlobal( true ); setCommunicating( false ); - setMass( true ); + setMassAndInertiaToInfinity(); setFinite( false ); // Checking the mass properties of the plane - WALBERLA_ASSERT_FLOAT_EQUAL( mass_ , real_c(0), "Mass of plane is not 0" ); + WALBERLA_ASSERT ( std::isinf(mass_), "Mass of plane is not infinity" ); WALBERLA_ASSERT_FLOAT_EQUAL( invMass_ , real_c(0), "Inverse mass of plane is not 0" ); - WALBERLA_ASSERT( I_ == Mat3(real_c(0)), "Moment of inertia of plane is not 0\n" << I_ ); - WALBERLA_ASSERT( Iinv_ == Mat3(real_c(0)), "Inverse moment of inertia of plane is not 0" ); + WALBERLA_ASSERT ( isinf(I_), "Moment of inertia of plane is not infinity\n" << I_ ); + WALBERLA_ASSERT_FLOAT_EQUAL( Iinv_ , Mat3(real_c(0)), "Inverse moment of inertia of plane is not 0" ); // Checking the plane normal // Since the plane constructor is never directly called but only used in a small number diff --git a/src/pe/rigidbody/PlaneFactory.cpp b/src/pe/rigidbody/PlaneFactory.cpp index f1bee615c9e357991f8ba4a8819c6896a41b4cab..2f780a12a1d1377fcf72b283d082e0e3a1320884 100644 --- a/src/pe/rigidbody/PlaneFactory.cpp +++ b/src/pe/rigidbody/PlaneFactory.cpp @@ -46,9 +46,6 @@ PlaneID createPlane( BodyStorage& globalStorage, id_t uid, Vec3 normal, const Ve PlaneID plane = new Plane( sid, uid, gpos, normal, normal*gpos, material ); - plane->setCommunicating( false ); - plane->setMass( true ); - globalStorage.add(plane); // Logging the successful creation of the plane diff --git a/src/pe/rigidbody/RigidBody.h b/src/pe/rigidbody/RigidBody.h index 8627cf49698d946810e946dc3962b42035e30af8..d0cdbdb374d1489388c975cbd15baa6b88cb2945 100644 --- a/src/pe/rigidbody/RigidBody.h +++ b/src/pe/rigidbody/RigidBody.h @@ -166,12 +166,12 @@ public: inline void resetSB() ; inline void setFinite ( const bool finite ); - inline void setMass ( bool infinite ); inline void setVisible ( bool visible ); inline void setPosition ( real_t px, real_t py, real_t pz ); inline void setPosition ( const Vec3& gpos ); inline void setOrientation( real_t r, real_t i, real_t j, real_t k ); inline void setOrientation( const Quat& q ); + inline void setMassAndInertiaToInfinity(); inline void setRelLinearVel ( real_t vx, real_t vy, real_t vz ); inline void setRelLinearVel ( const Vec3& lvel ); @@ -357,8 +357,10 @@ protected: virtual void rotateImpl ( const Quat& dq ); virtual void rotateAroundOriginImpl( const Quat& dq ); virtual void rotateAroundPointImpl ( const Vec3& point, const Quat& dq ); - virtual bool containsRelPointImpl ( real_t px, real_t py, real_t pz ) const; - virtual bool isSurfaceRelPointImpl ( real_t px, real_t py, real_t pz ) const; + virtual bool containsRelPointImpl ( real_t px, real_t py, real_t pz ) const; + virtual bool isSurfaceRelPointImpl ( real_t px, real_t py, real_t pz ) const; + + inline void setMassAndInertia ( const real_t mass, const Mat3& inertia ); inline void calcRelPosition(); //@} @@ -706,7 +708,7 @@ inline bool RigidBody::isFixed() const */ inline bool RigidBody::hasInfiniteMass() const { - return (isIdentical(invMass_, real_t(0))) && ( Iinv_.isZero() ); + return std::isinf(getMass()); } //************************************************************************************************* @@ -2313,39 +2315,30 @@ inline void RigidBody::setFinite( const bool finite ) //************************************************************************************************* -/*!\brief Setting the global position (the center of mass) of the rigid body fixed. - * - * \return void - * - * This function either fixes the global position (the center of mass) of a finite - * rigid body. If the body is contained in a superordinate body, fixing the contained - * body will also fix the global position of the superordinate body. - * Fixation is permanent and a fixed body cannot be unfixed anymore. +/*!\brief Sets mass and inertia of a rigid body. Also calculates inverse values. * - * In case of a MPI parallel simulation, changing the settings of a (local) rigid body - * on one process may invalidate the settings of the rigid body on another process. In order to - * synchronize all rigid bodies after local changes, the simulation has to be synchronized - * by the user. Note that any changes on remote rigid - * bodies are neglected and overwritten by the settings of the rigid body on its local process! + * \param mass mass to be set (may be infinity) + * \param inertia inertia to be set (if mass is infinity this one will be ignored) */ -inline void RigidBody::setMass( bool infinite ) +inline void RigidBody::setMassAndInertia( const real_t mass, const Mat3& inertia ) { - if (infinite) + if ( std::isinf( mass ) ) { // Adjusting the inverse mass and inverse moment of inertia + mass_ = std::numeric_limits::infinity(); + I_ = Mat3::makeDiagonalMatrix(std::numeric_limits::infinity()); invMass_ = real_c(0); Iinv_ = Mat3( real_c(0) ); - - // Setting the linear and angular velocity to zero - v_ = Vec3(0); - w_ = Vec3(0); } else { // Adjusting the inverse mass and inverse moment of inertia + mass_ = mass; + I_ = inertia; invMass_ = real_c(1) / mass_; Iinv_ = I_.getInverse(); } + if (hasSuperBody()) WALBERLA_LOG_WARNING("Changing the mass of a body contained in a Union is currently untested!!!"); // Signaling the fixation change to the superordinate body signalFixation(); } @@ -2555,6 +2548,19 @@ inline void RigidBody::setOrientation( const Quat& q ) } //************************************************************************************************* +//************************************************************************************************* +/*!\fn void RigidBody::setMassAndInertiaToInfinity( ) + * \brief Setting the mass to infinity. This will also make the inertia tensor infinite. + * + * \attention This cannot be undone! + */ +//************************************************************************************************* +inline void RigidBody::setMassAndInertiaToInfinity() +{ + setMassAndInertia( std::numeric_limits::infinity(), Mat3(std::numeric_limits::infinity()) ); +} +//************************************************************************************************* + //================================================================================================= @@ -3261,7 +3267,7 @@ inline Vec3 RigidBody::supportContactThreshold(const Vec3& /*d*/) const inline void RigidBody::fix() { setCommunicating( false ); - setMass( true ); + setMassAndInertia( getMass(), getInertia() ); // Signaling the fixation change to the superordinate body signalFixation(); diff --git a/src/pe/rigidbody/Sphere.cpp b/src/pe/rigidbody/Sphere.cpp index 8ea3a81a768d586f47ab77b3016cead0e6b85fb3..f4314e9bad63289046a92b42b073bc15d75a60bc 100644 --- a/src/pe/rigidbody/Sphere.cpp +++ b/src/pe/rigidbody/Sphere.cpp @@ -83,15 +83,15 @@ Sphere::Sphere( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const V q_ = q; // Setting the orientation R_ = q_.toRotationMatrix(); // Setting the rotation matrix - // Calculating the sphere mass - mass_ = calcMass( radius, Material::getDensity( material ) ); - invMass_ = real_c(1) / mass_; - - // Calculating the moment of inertia - calcInertia(); - setGlobal( global ); - setMass( infiniteMass ); + if (infiniteMass) + { + setMassAndInertia( std::numeric_limits::infinity(), Mat3(real_t(0)) ); + } else + { + auto mass = calcMass( radius, Material::getDensity( material ) ); + setMassAndInertia( mass, calcInertia( radius, mass ) ); + } setCommunicating( communicating ); setFinite( true ); @@ -194,20 +194,20 @@ void Sphere::print( std::ostream& os, const char* tab ) const << tab << " Linear velocity = " << getLinearVel() << "\n" << tab << " Angular velocity = " << getAngularVel() << "\n"; -// if( verboseMode ) -// { - os << tab << " Bounding box = " << getAABB() << "\n" - << tab << " Quaternion = " << getQuaternion() << "\n" - << tab << " Rotation matrix = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n" - << tab << " ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n" - << tab << " ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n"; - - os << std::setiosflags(std::ios::right) - << tab << " Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n" - << tab << " ( " << setw(9) << I_[3] << " , " << setw(9) << I_[4] << " , " << setw(9) << I_[5] << " )\n" - << tab << " ( " << setw(9) << I_[6] << " , " << setw(9) << I_[7] << " , " << setw(9) << I_[8] << " )\n" - << std::resetiosflags(std::ios::right); -// } + // if( verboseMode ) + // { + os << tab << " Bounding box = " << getAABB() << "\n" + << tab << " Quaternion = " << getQuaternion() << "\n" + << tab << " Rotation matrix = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n" + << tab << " ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n" + << tab << " ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n"; + + os << std::setiosflags(std::ios::right) + << tab << " Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n" + << tab << " ( " << setw(9) << I_[3] << " , " << setw(9) << I_[4] << " , " << setw(9) << I_[5] << " )\n" + << tab << " ( " << setw(9) << I_[6] << " , " << setw(9) << I_[7] << " , " << setw(9) << I_[8] << " )\n" + << std::resetiosflags(std::ios::right); + // } } //************************************************************************************************* diff --git a/src/pe/rigidbody/Sphere.h b/src/pe/rigidbody/Sphere.h index 536f7b7ce767e37ceac9cae875348dfce45b2ab8..d062e616c08c4770bb3533c232f8fd10e4dc8f24 100644 --- a/src/pe/rigidbody/Sphere.h +++ b/src/pe/rigidbody/Sphere.h @@ -125,6 +125,7 @@ public: static inline real_t calcVolume( real_t radius ); static inline real_t calcMass( real_t radius, real_t density ); static inline real_t calcDensity( real_t radius, real_t mass ); + static inline Mat3 calcInertia( const real_t mass, const real_t radius ); //@} //********************************************************************************************** @@ -141,7 +142,6 @@ protected: /*!\name Utility functions */ //@{ inline virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box - inline void calcInertia(); // Calculation of the moment of inertia //@} //********************************************************************************************** @@ -287,10 +287,9 @@ inline void Sphere::calcBoundingBox() * * \return void */ -inline void Sphere::calcInertia() +inline Mat3 Sphere::calcInertia( const real_t mass, const real_t radius ) { - I_[0] = I_[4] = I_[8] = real_c(0.4) * mass_ * radius_ * radius_; - Iinv_ = I_.getInverse(); + return Mat3::makeDiagonalMatrix( real_c(0.4) * mass * radius * radius ); } //************************************************************************************************* diff --git a/tests/pe/CheckVitalParameters.h b/tests/pe/CheckVitalParameters.h index 424ef961278f839a79b251a7846d6599e4ac8d46..2349294e4278b6326cae5733c32666a800792e1c 100644 --- a/tests/pe/CheckVitalParameters.h +++ b/tests/pe/CheckVitalParameters.h @@ -41,10 +41,19 @@ inline void checkVitalParameters(SphereID bd1, SphereID bd2) WALBERLA_CHECK_FLOAT_EQUAL(bd1->getRotation(), bd2->getRotation()); WALBERLA_CHECK_FLOAT_EQUAL(bd1->getAngularVel(), bd2->getAngularVel()); - WALBERLA_CHECK_FLOAT_EQUAL(bd1->getMass(), bd2->getMass()); - WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvMass(), bd2->getInvMass()); - WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInertia(), bd2->getInertia()); - WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvInertia(), bd2->getInvInertia()); + if (std::isinf(bd1->getMass())) + { + WALBERLA_CHECK ( std::isinf(bd2->getMass()) ); + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvMass(), bd2->getInvMass()); + WALBERLA_CHECK ( math::isinf(bd1->getBodyInertia()) ); + WALBERLA_CHECK ( math::isinf(bd2->getBodyInertia()) ); + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvInertia(), bd2->getInvInertia()); + } else { + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getMass(), bd2->getMass()); + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvMass(), bd2->getInvMass()); + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInertia(), bd2->getInertia()); + WALBERLA_CHECK_FLOAT_EQUAL(bd1->getInvInertia(), bd2->getInvInertia()); + } WALBERLA_CHECK_EQUAL(bd1->isGlobal(), bd2->isGlobal()); WALBERLA_CHECK_EQUAL(bd1->hasInfiniteMass(), bd2->hasInfiniteMass()); diff --git a/tests/pe/Union.cpp b/tests/pe/Union.cpp index 8d48dae60856923cd6d64f4a0f906c3b4efb5067..6cd0aec53e1e26b978b0085a67a04ae74b15fc0e 100644 --- a/tests/pe/Union.cpp +++ b/tests/pe/Union.cpp @@ -78,24 +78,26 @@ void SnowManFallingOnPlane() MaterialID iron = Material::find("iron"); UnionType* un = createUnion< boost::tuple >( *globalBodyStorage, forest->getBlockStorage(), storageID, 0, Vec3(5,5,5) ); SphereID sp1 = new Sphere( 10, 0, Vec3(5,5,1), Vec3(0,0,0), Quat(), real_t(1) , iron, false, true, false ); - SphereID sp2 = new Sphere( 11, 0, Vec3(6,5,2), Vec3(0,0,0), Quat(), real_t(1.5), iron, false, true, false ); + SphereID sp2 = new Sphere( 11, 0, Vec3(real_t(6.7),5,real_t(1.2)), Vec3(0,0,0), Quat(), real_t(1.1), iron, false, true, false ); un->add(sp1); un->add(sp2); - auto vtkOutput = make_shared(storageID, forest->getBlockStorage()) ; - auto vtkWriter = vtk::createVTKOutput_PointData(vtkOutput, "Bodies", 1, "vtk_out", "simulation_step", false, false); + auto distance = (sp1->getPosition() - sp2->getPosition()).length(); - for (unsigned int i = 0; i < 10000; ++i) + //auto vtkOutput = make_shared(storageID, forest->getBlockStorage()) ; + //auto vtkWriter = vtk::createVTKOutput_PointData(vtkOutput, "Bodies", 1, "vtk_out", "simulation_step", false, false); + + for (unsigned int i = 0; i < 1000; ++i) { - vtkWriter->write( true ); - cr.timestep( real_t(0.001) ); + //vtkWriter->write( true ); + cr.timestep( real_t(0.1) ); } //WALBERLA_CHECK_FLOAT_EQUAL( sp1->getLinearVel().length(), real_t(0) ); //WALBERLA_CHECK_FLOAT_EQUAL( sp2->getLinearVel().length(), real_t(0) ); WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( sp1->getPosition()[2], real_t(1) , real_t(0.001) ); - WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( sp2->getPosition()[2], real_t(1.5), real_t(0.001) ); - WALBERLA_CHECK_FLOAT_EQUAL( (sp1->getPosition() - sp2->getPosition()).length(), real_t(sqrt(2)) ); + WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( sp2->getPosition()[2], real_t(1.1), real_t(0.001) ); + WALBERLA_CHECK_FLOAT_EQUAL( (sp1->getPosition() - sp2->getPosition()).length(), distance ); //WALBERLA_LOG_DEVEL(un); }