Builder/BuilderPipe.cs
namespace Sandbox;
public sealed class BuilderPipe
{
/// <summary>Where the pipe's values sit in the Bend shape's parameters.</summary>
public const int ThicknessSlot = 0, RadiusSlot = 1, EndSlot = 2, NormalSlot = 5;
/// <summary>End normal components are stored scaled up by this, so parameter rounding keeps them precise.</summary>
public const float NormalScale = 1024f;
public const int Columns = 16;
const int Samples = 64;
const int MinRows = 4;
const int MaxRows = 32;
const float DegreesPerRow = 10f;
const float MaxDegreesPerRow = 20f;
public Vector3 End { get; }
public Vector3 EndNormal { get; }
public float Radius { get; }
public float Thickness { get; }
public int Rows { get; }
readonly Vector3[] _points;
readonly Vector3[] _normals;
readonly Vector3[] _binormals;
BuilderPipe( Vector3 end, Vector3 endNormal, float radius, float thickness )
{
End = end;
EndNormal = endNormal;
Radius = radius;
Thickness = thickness;
Rows = RowCount();
if ( Rows == 0 ) return;
_points = new Vector3[Rows + 1];
_normals = new Vector3[Rows + 1];
_binormals = new Vector3[Rows + 1];
var normal = Vector3.Up;
var tangent = Vector3.Forward;
for ( int i = 0; i <= Rows; i++ )
{
var t = (float)i / Rows;
var point = Bezier( t );
var nextTangent = Derivative( t ).Normal;
if ( i > 0 )
{
var step = point - _points[i - 1];
var stepLength = step.LengthSquared;
if ( stepLength > 0.000001f )
{
var reflectedNormal = normal - step * (2 / stepLength * Vector3.Dot( step, normal ));
var reflectedTangent = tangent - step * (2 / stepLength * Vector3.Dot( step, tangent ));
var correction = nextTangent - reflectedTangent;
var correctionLength = correction.LengthSquared;
normal = correctionLength > 0.000001f
? reflectedNormal - correction * (2 / correctionLength * Vector3.Dot( correction, reflectedNormal ))
: reflectedNormal;
}
}
tangent = nextTangent;
normal = (normal - tangent * Vector3.Dot( normal, tangent )).Normal;
_points[i] = point;
_normals[i] = normal;
_binormals[i] = Vector3.Cross( tangent, normal );
}
}
public static bool TryCreate( Vector3 end, Vector3 endNormal, float radius, float thickness, out BuilderPipe pipe )
{
pipe = null;
if ( end.IsNaN || end.IsInfinity || endNormal.IsNaN || endNormal.IsInfinity ) return false;
if ( !float.IsFinite( radius ) || !float.IsFinite( thickness ) || radius < 0.5f || thickness <= 0 ) return false;
if ( endNormal.IsNearZeroLength || end.Length < radius * 2 ) return false;
var candidate = new BuilderPipe( end, endNormal.Normal, radius, MathF.Min( thickness, radius * 0.95f ) );
if ( candidate.Rows == 0 || !candidate.IsClean() || candidate.Touches() ) return false;
pipe = candidate;
return true;
}
public static bool TryFromKey( BuilderShapeKey key, out BuilderPipe pipe )
{
var end = new Vector3( key.Parameter( EndSlot ), key.Parameter( EndSlot + 1 ), key.Parameter( EndSlot + 2 ) );
var normal = new Vector3( key.Parameter( NormalSlot ), key.Parameter( NormalSlot + 1 ), key.Parameter( NormalSlot + 2 ) ) / NormalScale;
return TryCreate( end, normal, key.Parameter( RadiusSlot ), key.Parameter( ThicknessSlot ), out pipe );
}
public static BuilderShapeKey? Validate( BuilderShapeKey key )
{
if ( !TryFromKey( key, out var pipe ) ) return null;
var size = pipe.Bounds.Size;
if ( size.x > BuilderShapeKey.MaxSize || size.y > BuilderShapeKey.MaxSize || size.z > BuilderShapeKey.MaxSize ) return null;
return key
.WithParameter( ThicknessSlot, pipe.Thickness )
.WithSize( size.ComponentMax( new Vector3( BuilderShapeKey.MinSize ) ) );
}
public Vector3 Point( int row, float around, float radius )
{
row = Math.Clamp( row, 0, Rows );
return _points[row] + (_normals[row] * MathF.Cos( around ) + _binormals[row] * MathF.Sin( around )) * radius;
}
public BuilderFace StartCap => Cap( 0, false );
public BuilderFace EndCap => Cap( Rows, true );
BuilderFace Cap( int row, bool forward )
{
var a = forward ? _normals[row] : _binormals[row];
var b = forward ? _binormals[row] : _normals[row];
var corner = _points[row] - (a + b) * Radius;
return new BuilderFace( corner, corner + a * (Radius * 2), corner + (a + b) * (Radius * 2), corner + b * (Radius * 2) );
}
public BBox Bounds
{
get
{
var min = new Vector3( float.MaxValue );
var max = new Vector3( float.MinValue );
for ( int row = 0; row <= Rows; row++ )
for ( int u = 0; u < Columns; u++ )
{
var p = Point( row, MathF.Tau * u / Columns, Radius );
min = min.ComponentMin( p );
max = max.ComponentMax( p );
}
return new BBox( min, max );
}
}
float Reach => End.Length * 0.5f;
Vector3 Bezier( float t )
{
var a = Vector3.Forward * Reach;
var b = End + EndNormal * Reach;
var s = 1 - t;
return a * (3 * s * s * t) + b * (3 * s * t * t) + End * (t * t * t);
}
Vector3 Derivative( float t )
{
var a = Vector3.Forward * Reach;
var b = End + EndNormal * Reach;
var s = 1 - t;
return a * (3 * s * s) + (b - a) * (6 * s * t) + (End - b) * (3 * t * t);
}
int RowCount()
{
var peak = 0f;
var previous = Derivative( 0 ).Normal;
for ( int i = 1; i <= Samples; i++ )
{
var tangent = Derivative( (float)i / Samples ).Normal;
peak = MathF.Max( peak, MathF.Acos( Math.Clamp( Vector3.Dot( previous, tangent ), -1f, 1f ) ) );
previous = tangent;
}
var degreesPerT = peak * Samples * 180f / MathF.PI;
if ( degreesPerT / MaxRows > MaxDegreesPerRow ) return 0;
return Math.Clamp( (int)MathF.Ceiling( degreesPerT / DegreesPerRow ), MinRows, MaxRows );
}
/// <summary>
/// Whether two parts of the tube that are further apart along the path than half a turn of its own size come
/// closer than its diameter.
/// </summary>
bool Touches()
{
Span<Vector3> points = stackalloc Vector3[Samples + 1];
Span<float> distances = stackalloc float[Samples + 1];
for ( int i = 0; i <= Samples; i++ )
{
points[i] = Bezier( (float)i / Samples );
distances[i] = i == 0 ? 0 : distances[i - 1] + points[i].Distance( points[i - 1] );
}
var apart = MathF.PI * Radius;
var clearance = Radius * 2 * 0.99f;
for ( int i = 0; i < Samples; i++ )
for ( int j = i + 2; j < Samples; j++ )
{
if ( distances[j] - distances[i + 1] <= apart ) continue;
if ( SegmentDistance( points[i], points[i + 1], points[j], points[j + 1] ) < clearance ) return true;
}
return false;
}
static float SegmentDistance( Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2 )
{
var d1 = q1 - p1;
var d2 = q2 - p2;
var r = p1 - p2;
var a = d1.LengthSquared;
var e = d2.LengthSquared;
var f = Vector3.Dot( d2, r );
float s, t;
if ( a <= 0.000001f && e <= 0.000001f ) return r.Length;
if ( a <= 0.000001f )
{
s = 0;
t = Math.Clamp( f / e, 0, 1 );
}
else
{
var c = Vector3.Dot( d1, r );
if ( e <= 0.000001f )
{
t = 0;
s = Math.Clamp( -c / a, 0, 1 );
}
else
{
var b = Vector3.Dot( d1, d2 );
var denominator = a * e - b * b;
s = denominator > 0.000001f ? Math.Clamp( (b * f - c * e) / denominator, 0, 1 ) : 0;
t = (b * s + f) / e;
if ( t < 0 )
{
t = 0;
s = Math.Clamp( -c / a, 0, 1 );
}
else if ( t > 1 )
{
t = 1;
s = Math.Clamp( (b - c) / a, 0, 1 );
}
}
}
return (p1 + d1 * s).Distance( p2 + d2 * t );
}
bool IsClean()
{
var previousPoint = Bezier( 0 );
var previousTangent = Derivative( 0 );
if ( previousTangent.IsNearZeroLength ) return false;
for ( int i = 1; i <= Samples; i++ )
{
var t = (float)i / Samples;
var point = Bezier( t );
var tangent = Derivative( t );
if ( tangent.IsNearZeroLength || Vector3.Dot( tangent, previousTangent ) <= 0 ) return false;
var turn = MathF.Acos( Math.Clamp( Vector3.Dot( tangent.Normal, previousTangent.Normal ), -1f, 1f ) );
if ( turn > 0.0001f && point.Distance( previousPoint ) / turn <= Radius * 1.05f ) return false;
previousPoint = point;
previousTangent = tangent;
}
return true;
}
}