//----------------------------------------------------------------------------- // File: HelloCenterOfMass.cpp // Class: None // Parent: None // Children: None // Purpose: Finds center of mass of particles //----------------------------------------------------------------------------- // The following are standard C/C++ header files. // If a filename is enclosed inside < > it means the header file is in the Include directory. // If a filename is enclosed inside " " it means the header file is in the current directory. #include // Character Types #include // Mathematical Constants #include // Variable Argument Lists #include // Standard Input/Output Functions #include // Utility Functions #include // String Operations #include // Signals (Contol-C + Unix System Calls) #include // Nonlocal Goto (For Control-C) #include // Time and Date information #include // Verify Program Assertion #include // Error Codes (Used in Unix system()) #include // Floating Point Constants #include // Implementation Constants #include // Standard Definitions #include // Exception handling (e.g., try, catch throw) //----------------------------------------------------------------------------- #include "SimTKsimbody.h" using namespace SimTK; using namespace std; //----------------------------------------------------------------------------- #define PI 3.141592653589793238462643 //----------------------------------------------------------------------------- // Prototypes for local functions (functions not called by code in other files) //----------------------------------------------------------------------------- bool CalculateMassProperties( void ); bool WriteStringToFile( const char outputString[], FILE *fptr ) { return fputs( outputString, fptr ) != 0; } bool WriteStringToScreen( const char outputString[] ) { return WriteStringToFile( outputString, stdout ); } bool WriteDoubleToFile( double x, int precision, FILE *fptr ); FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute ); //----------------------------------------------------------------------------- // The executable program starts here //----------------------------------------------------------------------------- int main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] ) { // Simulate the multibody system bool simulationSucceeded = CalculateMassProperties(); // Keep the screen displayed until the user presses the Enter key WriteStringToScreen( "\n\n Press Enter to terminate the program: " ); getchar(); // The value returned by the main function is the exit status of the program. // A normal program exit returns 0 (other return values usually signal an error). return simulationSucceeded == true ? 0 : 1; } //----------------------------------------------------------------------------- bool CalculateMassProperties( void ) { // Declare a multibody system (contains one or more force and matter sub-systems) MultibodySystem mbs; // 0. The ground's right-handed, orthogonal x,y,z unit vectors are directed with x horizontally right and y vertically upward. // 1. Create a gravity vector that is straight down (in the ground's frame) // 2. Create a uniform gravity sub-system // 3. Add the gravity sub-system to the multibody system Vec3 gravityVector( 0, -9.8, 0 ); UniformGravitySubsystem gravity( gravityVector ); mbs.addForceSubsystem( gravity ); // Create a matter sub-system (the atom) SimbodyMatterSubsystem atom; // Create the mass, center of mass, and inertia properties for the atom const Real massOfQ1 = 1.0; const Real massOfQ2 = 2.0; const Real massOfQ3 = 2.0; const Real massOfQ4 = 2.0; const Real massSum = massOfQ1 + massOfQ2 + massOfQ3 + massOfQ4; // The location of the atom's center of mass is a vector from the atom's // origin expressed in the "x, y, z"' unit vectors fixed in the atom's frame. // Example: The vector(0,0,0) locates the atom's center of mass at the atom's origin. // Example: The vector(1,0,0) locates the atom's center of mass 1 unit in the "x" direction from the atom's origin. const Vec3 atomCenterOfMassLocation( 0, 0, 0 ); // Create the atom's inertia matrix about its origin for the "x, y, z" unit vectors fixed in the atom's frame. const Vec3 positionOfQ1FromO( -1.0, 0.0, 0.0 ); const Vec3 positionOfQ2FromO( 0.0, 1.0, 0.0 ); const Vec3 positionOfQ3FromO( 1.0, 0.0, 0.0 ); const Vec3 positionOfQ4FromO( 0.0,-1.0, 0.0 ); const Vec3 positionOfCMFromO = ( massOfQ1*positionOfQ1FromO + massOfQ2*positionOfQ2FromO + massOfQ3*positionOfQ3FromO + massOfQ4*positionOfQ4FromO )/ massSum; const Vec3 positionOfQ1FromCM = positionOfQ1FromO - positionOfCMFromO; const Vec3 positionOfQ2FromCM = positionOfQ2FromO - positionOfCMFromO; const Vec3 positionOfQ3FromCM = positionOfQ3FromO - positionOfCMFromO; const Vec3 positionOfQ4FromCM = positionOfQ4FromO - positionOfCMFromO; Mat33 unitDyadic; unitDyadic[0][0] = 1.0; unitDyadic[0][1] = 0.0; unitDyadic[0][2] = 0.0; unitDyadic[1][0] = 0.0; unitDyadic[1][1] = 1.0; unitDyadic[1][2] = 0.0; unitDyadic[2][0] = 0.0; unitDyadic[2][1] = 0.0; unitDyadic[2][2] = 1.0; Mat33 inertiaOfQ1AboutO = massOfQ1*( unitDyadic*(dot(positionOfQ1FromO,positionOfQ1FromO)) - outer(positionOfQ1FromO,positionOfQ1FromO) ); Mat33 inertiaOfQ2AboutO = massOfQ2*( unitDyadic*(dot(positionOfQ2FromO,positionOfQ2FromO)) - outer(positionOfQ2FromO,positionOfQ2FromO) ); Mat33 inertiaOfQ3AboutO = massOfQ3*( unitDyadic*(dot(positionOfQ3FromO,positionOfQ3FromO)) - outer(positionOfQ3FromO,positionOfQ3FromO) ); Mat33 inertiaOfQ4AboutO = massOfQ4*( unitDyadic*(dot(positionOfQ4FromO,positionOfQ4FromO)) - outer(positionOfQ4FromO,positionOfQ4FromO) ); Mat33 inertiaOfSystemAboutO = inertiaOfQ1AboutO + inertiaOfQ2AboutO + inertiaOfQ3AboutO + inertiaOfQ4AboutO; Mat33 inertiaOfQ1AboutCM = massOfQ1*( unitDyadic*(dot(positionOfQ1FromCM,positionOfQ1FromCM)) - outer(positionOfQ1FromCM,positionOfQ1FromCM) ); Mat33 inertiaOfQ2AboutCM = massOfQ2*( unitDyadic*(dot(positionOfQ2FromCM,positionOfQ2FromCM)) - outer(positionOfQ2FromCM,positionOfQ2FromCM) ); Mat33 inertiaOfQ3AboutCM = massOfQ3*( unitDyadic*(dot(positionOfQ3FromCM,positionOfQ3FromCM)) - outer(positionOfQ3FromCM,positionOfQ3FromCM) ); Mat33 inertiaOfQ4AboutCM = massOfQ4*( unitDyadic*(dot(positionOfQ4FromCM,positionOfQ4FromCM)) - outer(positionOfQ4FromCM,positionOfQ4FromCM) ); Mat33 inertiaOfSystemAboutCM = inertiaOfQ1AboutCM + inertiaOfQ2AboutCM + inertiaOfQ3AboutCM + inertiaOfQ4AboutCM; // Open a file to write inertia matrix about O FILE *outputFile = FileOpenWithMessageIfCannotOpen( "HelloCenterOfMassAboutO.txt", "w" ); WriteStringToFile( " Inertia Matrix of System About O\n", outputFile ); WriteStringToFile( " Ixx Ixy Ixz\n", outputFile ); // Print results to file WriteDoubleToFile( inertiaOfSystemAboutO[0][0], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[0][1], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[0][2], 5, outputFile ); WriteStringToFile( "\n Iyx Iyy Iyz\n", outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[1][0], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[1][1], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[1][2], 5, outputFile ); WriteStringToFile( "\n Izx Izy Izz\n", outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[2][0], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[2][1], 5, outputFile ); WriteDoubleToFile( inertiaOfSystemAboutO[2][2], 5, outputFile ); // Open a file to write inertia matrix about CM FILE *outputFile2 = FileOpenWithMessageIfCannotOpen( "HelloCenterOfMassAboutCM.txt", "w" ); WriteStringToFile( " Inertia Matrix of System About CM\n", outputFile ); WriteStringToFile( " Ixx Ixy Ixz\n", outputFile2 ); // Print results to file WriteDoubleToFile( inertiaOfSystemAboutCM[0][0], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[0][1], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[0][2], 5, outputFile2 ); WriteStringToFile( "\n Iyx Iyy Iyz\n", outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[1][0], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[1][1], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[1][2], 5, outputFile2 ); WriteStringToFile( "\n Izx Izy Izz\n", outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[2][0], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[2][1], 5, outputFile2 ); WriteDoubleToFile( inertiaOfSystemAboutCM[2][2], 5, outputFile2 ); // Simulation completed properly return true; } //----------------------------------------------------------------------------- FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute ) { // Try to open the file FILE *Fptr1 = fopen( filename, attribute ); // If unable to open the file, issue a message if( !Fptr1 ) { WriteStringToScreen( "\n\n Unable to open the file: " ); WriteStringToScreen( filename ); WriteStringToScreen( "\n\n" ); } return Fptr1; } //----------------------------------------------------------------------------- bool WriteDoubleToFile( double x, int precision, FILE *fptr ) { // Ensure the precision (number of digits in the mantissa after the decimal point) makes sense. // Next, calculate the field width so it includes one extra space to the right of the number. if( precision < 0 || precision > 17 ) precision = 5; int fieldWidth = precision + 8; // Create the format specifier and print the number char format[20]; sprintf( format, " %%- %d.%dE", fieldWidth, precision ); return fprintf( fptr, format, x ) >= 0; }