kekobka/velox
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Valera
2025-07-18 16:05:48 +07:00
parent 55a178e8c5
commit 0905876b99
9 changed files with 347 additions and 752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
Code/Utils/PhysicsExtensions.cs
View File

@@ -28,44 +28,38 @@ public static class PhysicsExtensions
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.
/// 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>
/// <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 )
/// <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 )
return (Vector3.Zero, Vector3.Zero);
{
LinearVelocity = 0;
AngularVelocity = 0;
return;
}
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
Rotation bodyRot = physObj.PhysicsBody.Rotation;
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);
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>
@@ -74,34 +68,31 @@ public static class PhysicsExtensions
/// <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,
/// <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 )
Vector3 position,
out Vector3 LinearImpulse,
out Vector3 AngularImpulse )
{
if ( !physObj.IsValid() || !physObj.MotionEnabled )
{
return (Vector3.Zero, Vector3.Zero);
LinearImpulse = 0;
AngularImpulse = 0;
return;
}
// 1. Linear impulse is the same as the input impulse (conservation of momentum)
Vector3 linearImpulse = impulse;
Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
Rotation bodyRot = physObj.PhysicsBody.Rotation;
// 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);
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;
}
}