99 lines
3.3 KiB
C#
99 lines
3.3 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 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>
|
|
/// <param name="LinearVelocity">Linear velocity on center of mass (World frame)</param>
|
|
/// <param name="AngularVelocity">Angular velocity on center of mass (World frame)</param>
|
|
public static void CalculateVelocityOffset(
|
|
this Rigidbody physObj,
|
|
Vector3 impulse,
|
|
Vector3 position,
|
|
out Vector3 LinearVelocity,
|
|
out Vector3 AngularVelocity )
|
|
{
|
|
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
|
{
|
|
LinearVelocity = 0;
|
|
AngularVelocity = 0;
|
|
return;
|
|
}
|
|
|
|
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
|
|
Rotation bodyRot = physObj.PhysicsBody.Rotation;
|
|
|
|
Vector3 r = position - com;
|
|
Vector3 torque = Vector3.Cross( r, impulse );
|
|
Vector3 torqueLocal = bodyRot.Inverse * torque;
|
|
Vector3 angularVelocityLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
|
|
AngularVelocity = bodyRot * angularVelocityLocal;
|
|
LinearVelocity = impulse * (1 / physObj.Mass);
|
|
}
|
|
|
|
/// <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>
|
|
/// <param name="LinearImpulse">Linear impulse on center of mass (World frame)</param>
|
|
/// <param name="AngularImpulse">Angular impulse on center of mass (World frame)</param>
|
|
public static void CalculateForceOffset(
|
|
this Rigidbody physObj,
|
|
Vector3 impulse,
|
|
Vector3 position,
|
|
out Vector3 LinearImpulse,
|
|
out Vector3 AngularImpulse )
|
|
{
|
|
if ( !physObj.IsValid() || !physObj.MotionEnabled )
|
|
{
|
|
LinearImpulse = 0;
|
|
AngularImpulse = 0;
|
|
return;
|
|
}
|
|
|
|
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
|
|
Rotation bodyRot = physObj.PhysicsBody.Rotation;
|
|
|
|
Vector3 r = position - com;
|
|
Vector3 torque = Vector3.Cross( r, impulse );
|
|
Vector3 torqueLocal = bodyRot.Inverse * torque;
|
|
Vector3 angularImpulseLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
|
|
AngularImpulse = bodyRot * angularImpulseLocal;
|
|
LinearImpulse = impulse;
|
|
}
|
|
|
|
}
|