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 );