Game/Physics/PhysicsHold.cs

A physgun style grab helper. PhysicsHold drives a keyframed PhysicsBody through a control joint to pull a held Rigidbody toward a target transform, plus helpers for snapping the held object's rotation, clamping grab distance and collecting connected bodies through joints.

using Sandbox.Physics;

/// <summary>
/// Moves a rigidbody towards a target transform through a control joint, the way the physgun holds things.
/// Only the instance that simulates the body can move it, so run this there
/// </summary>
public sealed class PhysicsHold
{
	const float MaxLinearAcceleration = 700000.0f;

	Sandbox.Physics.ControlJoint _joint;
	PhysicsBody _body;
	Vector3 _massCenterOffset;

	/// <summary>
	/// True while there's a joint holding a body.
	/// </summary>
	public bool IsHolding => _joint is not null;

	/// <summary>
	/// Starts holding <paramref name="body"/> by <paramref name="grabPoint"/>, in world space. Does nothing if we're already holding.
	/// Lower <paramref name="damping"/> follows the target more tightly, 1 is critically damped.
	/// </summary>
	public void Hold( Rigidbody body, Vector3 grabPoint, float damping = 4.0f )
	{
		if ( IsHolding ) return;

		var physicsBody = body.PhysicsBody;
		_body ??= new PhysicsBody( physicsBody.World ) { BodyType = PhysicsBodyType.Keyframed, AutoSleep = false };

		var bodyTransform = body.WorldTransform.WithScale( 1.0f );
		_massCenterOffset = physicsBody.LocalMassCenter - bodyTransform.PointToLocal( grabPoint );

		var point1 = new PhysicsPoint( _body );
		var point2 = new PhysicsPoint( physicsBody, physicsBody.LocalMassCenter );
		var maxForce = physicsBody.Mass * MaxLinearAcceleration;

		var inertia = physicsBody.Inertia;
		var maxTorque = MaxLinearAcceleration * MathF.Sqrt( physicsBody.Mass * MathF.Max( inertia.x, MathF.Max( inertia.y, inertia.z ) ) );

		_joint = PhysicsJoint.CreateControl( point1, point2 );
		_joint.LinearSpring = new PhysicsSpring( 32, damping, maxForce );
		_joint.AngularSpring = new PhysicsSpring( 64, damping, maxTorque );
	}

	public void Move( Transform target )
	{
		if ( _body is null ) return;
		_body.Transform = target.WithPosition( target.PointToWorld( _massCenterOffset ) );
	}

	public void Release()
	{
		_joint?.Remove();
		_joint = null;

		_body?.Remove();
		_body = null;
	}

	public static Rotation Stabilize( Rotation rotation )
	{
		var forward = rotation.Forward.WithZ( 0 );
		var up = rotation.Up.WithZ( 0 );

		// looking up the top of the view points backwards, looking down it points forwards
		var heading = forward.LengthSquared >= up.LengthSquared ? forward : (rotation.Forward.z > 0 ? -up : up);
		return Rotation.FromYaw( MathX.RadianToDegree( MathF.Atan2( heading.y, heading.x ) ) );
	}

	public static float ClampGrabDistance( Rigidbody body, Vector3 point, Transform eye, float distance, float min = 50.0f )
	{
		distance = MathF.Max( 0.0f, distance );
		var closest = body.FindClosestPoint( eye.Position );
		var along = distance + Vector3.Dot( closest - point, eye.Rotation.Forward );
		return along < min ? distance + (min - along) : distance;
	}

	public static void GetConnectedBodies( GameObject source, HashSet<Rigidbody> result )
	{
		foreach ( var rb in source.Root.Components.GetAll<Rigidbody>() )
		{
			if ( !result.Add( rb ) ) continue;

			foreach ( var joint in rb.Joints )
			{
				if ( joint.Object1 != null ) GetConnectedBodies( joint.Object1, result );
				if ( joint.Object2 != null ) GetConnectedBodies( joint.Object2, result );
			}
		}
	}
}

/// <summary>
/// Rotating a held object with the mouse and movement keys while use is held, snapping to 45 degrees when running.
/// </summary>
public sealed class HoldRotation
{
	public static float KeyboardRotateSpeed => 240.0f;

	Rotation _spin;
	Rotation _snap;
	bool _isSnapping;

	public void Reset( Rotation offset )
	{
		_spin = offset;
		_snap = offset;
	}

	public void UpdateSnapping()
	{
		var isSnapping = Input.Down( "run" ) || Input.Down( "walk" );
		if ( !isSnapping && _isSnapping ) _spin = _snap;
		_isSnapping = isSnapping;
	}

	public Rotation Rotate( Rotation frame, Vector3 move, bool pitchWithKeys )
	{
		var look = Input.AnalogLook * -1;
		var snapAngle = Input.Down( "walk" ) ? 15.0f : 45.0f;

		if ( _isSnapping )
		{
			if ( MathF.Abs( look.yaw ) > MathF.Abs( look.pitch ) ) look.pitch = 0;
			else look.yaw = 0;
		}

		_spin = Rotation.From( look ) * _spin;
		var spinRotation = _spin;

		if ( pitchWithKeys && MathF.Abs( move.x ) > 0.001f )
			spinRotation = Rotation.From( new Angles( move.x * KeyboardRotateSpeed * Time.Delta, 0f, 0f ) ) * spinRotation;

		if ( MathF.Abs( move.y ) > 0.001f )
		{
			var worldRotation = frame * spinRotation;
			worldRotation = Rotation.FromYaw( move.y * KeyboardRotateSpeed * Time.Delta ) * worldRotation;
			spinRotation = frame.Inverse * worldRotation;
		}

		_spin = spinRotation;

		if ( _isSnapping )
		{
			spinRotation = frame * spinRotation;

			var angles = spinRotation.Angles();
			spinRotation = angles.SnapToGrid( snapAngle );

			spinRotation = frame.Inverse * spinRotation;
		}

		// save snap rotation so it can be applied after snap has finished
		_snap = spinRotation;
		return spinRotation;
	}
}