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)
+ );
+ }
///
/// Calculates the linear and angular velocities on the center of mass for an offset impulse.
///
@@ -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);
}
///
@@ -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 );