Доломал CalculateVelocityOffset (оно и не работало)

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 r = position - physObj.MassCenter;
-		Vector3 relativePosition = position - centerOfMass;
-		Vector3 torque = relativePosition.Cross( impulse );
 
-		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(
+		var InverseInertiaDiagLocal = physObj.Inertia.Inverse;
-			localTorque.x * localInverseInertia.x,
-			localTorque.y * localInverseInertia.y,
-			localTorque.z * localInverseInertia.z );
 
-		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 );