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

Доломал CalculateVelocityOffset (оно и не работало)

Browse Source
This commit is contained in:
Valera 2025-06-12 22:55:58 +07:00
parent c7c17c1778
commit 316c28b86d
1 changed files with 41 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Code/Utils/PhysicsExtensions.cs
View File

@ -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)
);
}
/// <summary>
/// Calculates the linear and angular velocities on the center of mass for an offset impulse.
/// </summary>
@ -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);
}
/// <summary>
@ -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 );