108 lines
3.8 KiB
C#
108 lines
3.8 KiB
C#
|
||
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>
|
||
/// <param name="physObj">The physics object</param>
|
||
/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
|
||
/// <param name="position">The location of the impulse in world coordinates</param>
|
||
/// <returns>
|
||
/// Vector1: Linear velocity from the impulse (World frame)
|
||
/// Vector2: Angular velocity from the impulse (Local frame)
|
||
/// </returns>
|
||
public static (Vector3 LinearVelocity, Vector3 AngularVelocity) CalculateVelocityOffset( this PhysicsBody physObj, Vector3 impulse, Vector3 position )
|
||
{
|
||
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
||
return (Vector3.Zero, Vector3.Zero);
|
||
|
||
|
||
Vector3 linearVelocity = impulse / physObj.Mass;
|
||
|
||
Vector3 r = position - physObj.MassCenter;
|
||
|
||
|
||
// Calculate torque impulse in world frame: τ = r × impulse
|
||
Vector3 torqueImpulseWorld = r.Cross( impulse );
|
||
Rotation worldToLocal = physObj.Rotation.Inverse;
|
||
Vector3 torqueImpulseLocal = torqueImpulseWorld.Transform( worldToLocal );
|
||
|
||
var InverseInertiaDiagLocal = physObj.Inertia.Inverse;
|
||
|
||
// 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>
|
||
/// Calculates the linear and angular impulses on the object's center of mass for an offset impulse.
|
||
/// </summary>
|
||
/// <param name="physObj">The physics object</param>
|
||
/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
|
||
/// <param name="position">The location of the impulse in world coordinates</param>
|
||
/// <returns>
|
||
/// Vector1: Linear impulse on center of mass (World frame)
|
||
/// Vector2: Angular impulse on center of mass (Local frame)
|
||
/// </returns>
|
||
public static (Vector3 LinearImpulse, Vector3 AngularImpulse) CalculateForceOffset(
|
||
this PhysicsBody physObj,
|
||
Vector3 impulse,
|
||
Vector3 position )
|
||
{
|
||
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
||
{
|
||
return (Vector3.Zero, Vector3.Zero);
|
||
}
|
||
|
||
// 1. Linear impulse is the same as the input impulse (conservation of momentum)
|
||
Vector3 linearImpulse = impulse;
|
||
|
||
// 2. Calculate angular impulse (torque) from the offset 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 );
|
||
|
||
// Convert angular impulse to local space (since we'll use it with LocalInertia)
|
||
Rotation bodyRotation = physObj.Transform.Rotation;
|
||
Vector3 localAngularImpulse = bodyRotation.Inverse * worldAngularImpulse;
|
||
|
||
return (linearImpulse, localAngularImpulse);
|
||
}
|
||
|
||
}
|