79 lines
2.8 KiB
C#
79 lines
2.8 KiB
C#
|
||
using Sandbox;
|
||
|
||
namespace VeloX;
|
||
public static class PhysicsExtensions
|
||
{
|
||
/// <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 centerOfMass = 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;
|
||
|
||
Vector3 localAngularVelocity = new(
|
||
localTorque.x * localInverseInertia.x,
|
||
localTorque.y * localInverseInertia.y,
|
||
localTorque.z * localInverseInertia.z );
|
||
|
||
return (linearVelocity, localAngularVelocity);
|
||
}
|
||
|
||
/// <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);
|
||
}
|
||
|
||
}
|