From 316c28b86dbf3f688994a16869fe8295167dbd54 Mon Sep 17 00:00:00 2001
From: Valera <108022376+kekobka@users.noreply.github.com>
Date: Thu, 12 Jun 2025 22:55:58 +0700
Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=BB=D0=BE=D0=BC=D0=B0=D0=BB=20Ca?=
 =?UTF-8?q?lculateVelocityOffset=20(=D0=BE=D0=BD=D0=BE=20=D0=B8=20=D0=BD?=
 =?UTF-8?q?=D0=B5=20=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=B0=D0=BB=D0=BE)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Code/Utils/PhysicsExtensions.cs | 53 +++++++++++++++++++++++++--------
 1 file changed, 41 insertions(+), 12 deletions(-)

diff --git a/Code/Utils/PhysicsExtensions.cs b/Code/Utils/PhysicsExtensions.cs
index 27aace7..b14b0a1 100644
--- a/Code/Utils/PhysicsExtensions.cs
+++ b/Code/Utils/PhysicsExtensions.cs
@@ -1,9 +1,33 @@
 
 using Sandbox;
+using System;
+using System.Numerics;
 
 namespace VeloX;
 public static class PhysicsExtensions
 {
+	public static Vector3 Transform( this Vector3 value, Quaternion rotation )
+	{
+		float x2 = rotation.X + rotation.X;
+		float y2 = rotation.Y + rotation.Y;
+		float z2 = rotation.Z + rotation.Z;
+
+		float wx2 = rotation.W * x2;
+		float wy2 = rotation.W * y2;
+		float wz2 = rotation.W * z2;
+		float xx2 = rotation.X * x2;
+		float xy2 = rotation.X * y2;
+		float xz2 = rotation.X * z2;
+		float yy2 = rotation.Y * y2;
+		float yz2 = rotation.Y * z2;
+		float zz2 = rotation.Z * z2;
+
+		return new Vector3(
+			value.x * (1.0f - yy2 - zz2) + value.y * (xy2 - wz2) + value.z * (xz2 + wy2),
+			value.x * (xy2 + wz2) + value.y * (1.0f - xx2 - zz2) + value.z * (yz2 - wx2),
+			value.x * (xz2 - wy2) + value.y * (yz2 + wx2) + value.z * (1.0f - xx2 - yy2)
+		);
+	}
 	/// <summary>
 	/// Calculates the linear and angular velocities on the center of mass for an offset impulse.
 	/// </summary>
@@ -22,21 +46,26 @@ public static class PhysicsExtensions
 
 		Vector3 linearVelocity = impulse / physObj.Mass;
 
-		Vector3 centerOfMass = physObj.MassCenter;
-		Vector3 relativePosition = position - centerOfMass;
-		Vector3 torque = relativePosition.Cross( impulse );
+		Vector3 r = position - physObj.MassCenter;
 
-		Rotation bodyRotation = physObj.Transform.Rotation;
-		Vector3 localTorque = bodyRotation.Inverse * torque;
 
-		Vector3 localInverseInertia = physObj.Inertia.Inverse;
+		// Calculate torque impulse in world frame: τ = r × impulse
+		Vector3 torqueImpulseWorld = r.Cross( impulse );
+		Rotation worldToLocal = physObj.Rotation.Inverse;
+		Vector3 torqueImpulseLocal = torqueImpulseWorld.Transform( worldToLocal );
 
-		Vector3 localAngularVelocity = new(
-			localTorque.x * localInverseInertia.x,
-			localTorque.y * localInverseInertia.y,
-			localTorque.z * localInverseInertia.z );
+		var InverseInertiaDiagLocal = physObj.Inertia.Inverse;
 
-		return (linearVelocity, localAngularVelocity);
+		// Compute angular velocity change in rad/s (local frame)
+		Vector3 angularVelocityRadLocal = new(
+			InverseInertiaDiagLocal.x * torqueImpulseLocal.x,
+			InverseInertiaDiagLocal.y * torqueImpulseLocal.y,
+			InverseInertiaDiagLocal.z * torqueImpulseLocal.z
+		);
+		const float radToDeg = 180f / MathF.PI;
+		Vector3 angularVelocityDegLocal = angularVelocityRadLocal * radToDeg;
+
+		return (linearVelocity, angularVelocityDegLocal);
 	}
 
 	/// <summary>
@@ -63,7 +92,7 @@ public static class PhysicsExtensions
 		Vector3 linearImpulse = impulse;
 
 		// 2. Calculate angular impulse (torque) from the offset force
-		// τ = r × F (cross product of position relative to COM and force)
+		// τ = r * F (cross product of position relative to COM and force)
 		Vector3 centerOfMass = physObj.MassCenter;
 		Vector3 relativePosition = position - centerOfMass;
 		Vector3 worldAngularImpulse = relativePosition.Cross( impulse );