Change of angular velocity integration method.

.gitignore

@@ -26,6 +26,10 @@ Thumbs.db
 #####################
 *.*sw*
 
+# LSP Client file
+compile_commands.json
+.cache/
+
 # documentation
 #####################
 documentation/API/html/

README.md

@@ -48,6 +48,10 @@
 
 You can find the user manual and the Doxygen API documentation <a href="https://www.reactphysics3d.com/documentation" target="_blank">here</a>.
 
+Instructions how to build and install library can be found in the file
+<a href="https://www.reactphysics3d.com/documentation/UserDocumentation.md"
+target="_blank">UserDocumentation.md</a>.
+
 ## Branches
 
 The "master" branch always contains the last released version of the library and some possible hot fixes. This is the most stable version. On the other side,

src/systems/DynamicsSystem.cpp

@@ -134,8 +134,31 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
         // Integrate the external force to get the new velocity of the body
         mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mInverseMasses[i] *
                                                                mRigidBodyComponents.mLinearLockAxisFactors[i] * mRigidBodyComponents.mExternalForces[i];
-        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mAngularLockAxisFactors[i] *
-                                                                (mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]);
+
+
+        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity; // old part of angular velocity 
+        {
+          Entity & entity = mRigidBodyComponents.mBodiesEntities[i];
+          const Matrix3x3 & B2W 
+              = mTransformComponents.getTransform(entity).getOrientation().getMatrix();
+          const Matrix3x3 W2B = B2W.getTranspose();
+
+          // inertia in local CS
+          Vector3 & I = mRigidBodyComponents.mLocalInertiaTensors[i];
+          // torque in local CS
+          Vector3 L =  W2B * mRigidBodyComponents.mExternalTorques[i];
+          // angular velocity in local CS
+          Vector3 om = W2B * mRigidBodyComponents.getAngularVelocity(entity);
+          Vector3 eps; // angular acceleration in local CS
+          // Euler equation
+          eps.x = L.x + (I.y-I.z)*om.y*om.z;
+          eps.y = L.y + (I.z-I.x)*om.z*om.x;
+          eps.z = L.z + (I.x-I.y)*om.x*om.y;
+          eps = eps/I;
+
+          mRigidBodyComponents.mConstrainedAngularVelocities[i] +=
+              timeStep * mRigidBodyComponents.mAngularLockAxisFactors[i] * (B2W * eps);
+        }
     }
 
     // Apply gravity force

test/CMakeLists.txt

@@ -31,6 +31,7 @@ set (RP3D_TESTS_HEADERS
     "tests/mathematics/TestVector2.h"
     "tests/mathematics/TestVector3.h"
     "tests/engine/TestRigidBody.h"
+    "tests/systems/TestDynamicSystem.h"
     "tests/utils/TestQuickHull.h"
 )
 

test/main.cpp

@@ -48,6 +48,7 @@
 #include "tests/containers/TestDeque.h"
 #include "tests/containers/TestStack.h"
 #include "tests/engine/TestRigidBody.h"
+#include "tests/systems/TestDynamicSystem.h"
 #include "tests/utils/TestQuickHull.h"
 
 using namespace reactphysics3d;
@@ -94,6 +95,7 @@ int main() {
     // ---------- Engine tests ---------- //
 
     testSuite.addTest(new TestRigidBody("RigidBody"));
+    testSuite.addTest(new TestDynamicSystem("Motion Integration"));
 
     // Run the tests
     testSuite.run();

test/tests/systems/TestDynamicSystem.h

@@ -0,0 +1,140 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2025 Grzegorz Szczodrzec
+* based on examples provided by library
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef TEST_DYNAMICSYSTEM_H
+#define TEST_DYNAMICSYSTEM_H
+// Libraries
+#include <reactphysics3d/reactphysics3d.h>
+#include "Test.h"
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+/**
+ * Unit test for the DynamicSystem class.
+ */
+
+class TestDynamicSystem : public Test {
+private:
+  PhysicsCommon mPhysicsCommon;
+  PhysicsWorld* mWorld = nullptr;
+  RigidBody* mRigidBody1 = nullptr;
+  RigidBody* mRigidBody2 = nullptr;
+
+public:
+  // ---------- Methods ---------- //
+  /// Constructor
+  TestDynamicSystem(const std::string& name);
+
+  /// Destructor 
+  virtual ~TestDynamicSystem();
+
+  /// Run the tests
+  virtual void run() override { testMotionIntegration(); }
+
+  // @brief integerates motion of Rigid Body and checks if angular momentum 
+  // remains constant.
+  //
+  // In absence of gravity and without colisions there is no source of torque.
+  // Without torque angular momentum have to be constant.
+  void testMotionIntegration();
+
+  Vector3 getAngularMomentum(RigidBody* pBody);
+};
+
+inline
+TestDynamicSystem::TestDynamicSystem(const std::string& name) : Test(name) {
+
+  mWorld = mPhysicsCommon.createPhysicsWorld();
+  mWorld->setIsGravityEnabled(false); 
+
+/*
+  Trying to make simulation of rotating stick to be close to reality. 
+  wooden stick 1m in length 2x2cm of cross-section
+  rho = 600 kg/m^3;
+  m=rho*a*b*c;
+  I = m(a^2+b^2)/12;
+ */
+  const decimal rho = 600; // kg/m^3
+  const decimal edge = 0.02; // 2 cm
+  const decimal stickLen = 1; // 1 m
+  // Create a rigid body in the world CS
+  Vector3 position(0, 2.0, 0);
+  Quaternion orientation = Quaternion::identity();
+  Transform transform(position, orientation);
+
+  mRigidBody1 = mWorld->createRigidBody(transform);
+
+  decimal mass = rho * edge * edge * stickLen;
+  mRigidBody1->setMass(mass);
+  { // inertia
+    decimal weak = mass * 2 * edge*edge /12;
+    decimal strong =  mass * (edge*edge + stickLen*stickLen)/12;
+    // local X is along stick
+    mRigidBody1->setLocalInertiaTensor( Vector3(weak, strong, strong) );
+  }
+  // kinematics
+  {
+    decimal omegaW = 1.0 / (stickLen/2);
+    mRigidBody1->setAngularVelocity(Vector3(0.1*omegaW, omegaW , 0));
+  }
+}
+
+inline 
+void TestDynamicSystem::testMotionIntegration(){
+
+    Vector3 initialAngularMomentum =  getAngularMomentum(mRigidBody1);
+    const decimal timeStep = 1.e-2 / 60.0f;
+	// Step the simulation a few steps
+	for (int i=0; i < 400; i++) {
+	  mWorld->update(timeStep);
+    }
+    Vector3 finalAngularMomentum =  getAngularMomentum(mRigidBody1);
+
+    rp3d_test(Vector3::approxEqual(initialAngularMomentum, finalAngularMomentum)); 
+}
+
+inline 
+Vector3 TestDynamicSystem::getAngularMomentum(RigidBody* pBody){
+  const Vector3 & Inertia = pBody->getLocalInertiaTensor();
+  const Vector3 & omegaW = pBody ->getAngularVelocity();
+  const Matrix3x3 & B2W = pBody->getTransform().getOrientation().getMatrix();
+
+  Vector3 omegaB = B2W.getTranspose() * omegaW;
+
+  Vector3 angMomentum = B2W * ( omegaB * Inertia ); // elwise product
+  return angMomentum;
+}
+
+inline TestDynamicSystem::~TestDynamicSystem(){
+  if(mRigidBody1)
+    mWorld->destroyRigidBody(mRigidBody1);
+  if(mRigidBody2)
+    mWorld->destroyRigidBody(mRigidBody2);
+}
+
+
+} // namespace
+#endif // TEST_DYNAMICSYSTEM_H