481 lines
12 KiB
C#
481 lines
12 KiB
C#
using Godot;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using System;
|
|
|
|
namespace Rokojori
|
|
{
|
|
public class FresnetFrame
|
|
{
|
|
public Vector3 tangent;
|
|
public Vector3 normal;
|
|
public Vector3 binormal;
|
|
}
|
|
|
|
public abstract class Curve3
|
|
{
|
|
float _tangentSamplingRange = Mathf.Pow( 10f, -8f );
|
|
public abstract Vector3 PositionAt( float t );
|
|
|
|
public virtual Quaternion RotationAt( float t )
|
|
{
|
|
return RotationFromTangent( t );
|
|
}
|
|
|
|
|
|
public Quaternion RotationFromTangent( float t )
|
|
{
|
|
// RJLog.Log( "Sampling Rotation From Tangent" );
|
|
var gradient = TangentAt( t );
|
|
return Math3D.LookRotation( gradient, Vector3.Up );
|
|
}
|
|
|
|
public virtual Pose PoseAt( float t )
|
|
{
|
|
return Pose.Create( PositionAt( t ), RotationAt( t ) );
|
|
}
|
|
|
|
public virtual Pose SmoothedPoseAt( float t, float stepSize = 0.01f, int steps = 1, float fallOff = 0.5f )
|
|
{
|
|
var poses = new List<Pose>();
|
|
|
|
poses.Add( PoseAt( t ) );
|
|
|
|
var weigths = new List<float>();
|
|
|
|
var weight = 1f;
|
|
weigths.Add( weight );
|
|
|
|
var sumWeigths = weight;
|
|
|
|
for ( int i = 0; i < steps; i++ )
|
|
{
|
|
var poseBefore = PoseAt( t - stepSize * i );
|
|
var poseAfter = PoseAt( t + stepSize * i );
|
|
|
|
poses.Add( poseBefore );
|
|
poses.Add( poseAfter );
|
|
|
|
weight *= fallOff;
|
|
weigths.Add( weight );
|
|
weigths.Add( weight );
|
|
|
|
sumWeigths += weight * 2;
|
|
}
|
|
|
|
for ( int i = 0; i < weigths.Count; i++ )
|
|
{
|
|
weigths[ i ] = weigths[ i ] / sumWeigths;
|
|
}
|
|
|
|
return Pose.Merge( poses, weigths );
|
|
}
|
|
|
|
public virtual void SampleMultiple( int numSamples, List<Vector3> output )
|
|
{
|
|
SampleMultiple( new Range( 0, 1 ), numSamples, output );
|
|
}
|
|
|
|
public virtual void SampleMultiple( Range range, int numSamples, List<Vector3> output )
|
|
{
|
|
var diff = range.length / ( numSamples - 1 ) ;
|
|
|
|
for ( var i = 0 ; i < numSamples; i++ )
|
|
{
|
|
var t = range.min + i * diff;
|
|
output.Add( PositionAt( t ) );
|
|
}
|
|
}
|
|
|
|
public virtual Path2 SampleXZPath( int numSamples )
|
|
{
|
|
return SampleXZPath( new Range( 0, 1 ), numSamples );
|
|
}
|
|
|
|
public virtual Path2 SampleXZPath( Range range, int numSamples )
|
|
{
|
|
var diff = range.length / ( numSamples - 1 ) ;
|
|
var points = new List<Vector2>();
|
|
|
|
for ( var i = 0 ; i < numSamples; i++ )
|
|
{
|
|
var t = range.min + i * diff;
|
|
var p = PositionAt( t );
|
|
points.Add( new Vector2( p.X, p.Z ) );
|
|
|
|
}
|
|
|
|
return new Path2( points );
|
|
}
|
|
|
|
public virtual Path2 SampleXYPath( int numSamples )
|
|
{
|
|
return SampleXYPath( new Range( 0, 1 ), numSamples );
|
|
}
|
|
|
|
public virtual Path2 SampleXYPath( Range range, int numSamples )
|
|
{
|
|
var diff = range.length / ( numSamples - 1 ) ;
|
|
var points = new List<Vector2>();
|
|
|
|
for ( var i = 0 ; i < numSamples; i++ )
|
|
{
|
|
var t = range.min + i * diff;
|
|
var p = PositionAt( t );
|
|
points.Add( new Vector2( p.X, p.Y ) );
|
|
|
|
}
|
|
|
|
return new Path2( points );
|
|
}
|
|
|
|
public const int CurveLengthDivisions = 20;
|
|
|
|
List<float> _curveLengths = null;
|
|
|
|
public void ClearCurveLengths()
|
|
{
|
|
_curveLengths = null;
|
|
}
|
|
|
|
|
|
List<float> _getCurveLengths( int divisions = CurveLengthDivisions )
|
|
{
|
|
|
|
if ( _curveLengths != null && _curveLengths.Count == ( divisions + 1 ) )
|
|
{
|
|
return _curveLengths;
|
|
}
|
|
|
|
|
|
_curveLengths = new List<float>();
|
|
_curveLengths.Add( 0f );
|
|
|
|
var before = PositionAt( 0 );
|
|
var sum = 0f;
|
|
|
|
for ( int i = 1; i <= divisions; i ++ ) {
|
|
|
|
var p = PositionAt( (float) i / divisions );
|
|
sum += p.DistanceTo( before );
|
|
|
|
_curveLengths.Add( sum );
|
|
before = p;
|
|
|
|
}
|
|
|
|
return _curveLengths;
|
|
}
|
|
|
|
public float UtoT( float normalizedCurveLength, int numCurveLengths = -1 )
|
|
{
|
|
return ComputeTforNormalizedCurveLength( normalizedCurveLength, numCurveLengths );
|
|
}
|
|
|
|
public float ComputeTforNormalizedCurveLength( float normalizedCurveLength, int numCurveLengths = -1 )
|
|
{
|
|
|
|
var curveLengths = numCurveLengths < 0 ? _getCurveLengths() : _getCurveLengths( numCurveLengths );
|
|
|
|
var i = 0;
|
|
numCurveLengths = curveLengths.Count;
|
|
|
|
var targetCurveLength = normalizedCurveLength * curveLengths[ numCurveLengths - 1 ];
|
|
|
|
var low = 0;
|
|
var high = numCurveLengths - 1;
|
|
var comparison = 0f;
|
|
|
|
while ( low <= high )
|
|
{
|
|
i = Mathf.FloorToInt( low + ( high - low ) / 2 );
|
|
|
|
comparison = curveLengths[ i ] - targetCurveLength;
|
|
|
|
if ( comparison < 0 )
|
|
{
|
|
low = i + 1;
|
|
}
|
|
else if ( comparison > 0 )
|
|
{
|
|
high = i - 1;
|
|
}
|
|
else
|
|
{
|
|
high = i;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
i = high;
|
|
|
|
if ( curveLengths[ i ] == targetCurveLength )
|
|
{
|
|
return i / ( numCurveLengths - 1 );
|
|
}
|
|
|
|
|
|
var lengthBefore = curveLengths[ i ];
|
|
var lengthAfter = curveLengths[ i + 1 ];
|
|
|
|
var segmentLength = lengthAfter - lengthBefore;
|
|
|
|
var segmentFraction = ( targetCurveLength - lengthBefore ) / segmentLength;
|
|
|
|
|
|
var t = ( i + segmentFraction ) / ( numCurveLengths - 1 );
|
|
|
|
return t;
|
|
|
|
}
|
|
|
|
public virtual Vector3 TangentAt( float t )
|
|
{
|
|
return TangentAt( t, _tangentSamplingRange );
|
|
}
|
|
|
|
public virtual Vector3 TangentAt( float t, float samplingRange )
|
|
{
|
|
return PositionAt( t + samplingRange ) - PositionAt( t - samplingRange );
|
|
}
|
|
|
|
public virtual float ComputeLength( int numSamples )
|
|
{
|
|
return ComputeSubLength( Range.Of_01 , numSamples );
|
|
}
|
|
|
|
public List<FresnetFrame> ComputeFresnetFrames( int segments, bool closed )
|
|
{
|
|
var frames = _GetFresnetFramesWithTangents( segments, closed );
|
|
|
|
_ComputeInitialNormalVector( frames );
|
|
_ComputeNormalsAndBinormals( frames, closed );
|
|
|
|
return frames;
|
|
}
|
|
|
|
List<FresnetFrame> _GetFresnetFramesWithTangents( int segments, bool closed )
|
|
{
|
|
var frames = new List<FresnetFrame>();
|
|
|
|
for ( int i = 0; i <= segments; i++ )
|
|
{
|
|
var fresnetFrame = new FresnetFrame();
|
|
var u = (float) i / segments;
|
|
var t = ComputeTforNormalizedCurveLength( u );
|
|
fresnetFrame.tangent = TangentAt( t ) ;
|
|
frames.Add( fresnetFrame );
|
|
}
|
|
|
|
return frames;
|
|
}
|
|
|
|
void _ComputeInitialNormalVector( List<FresnetFrame> frames )
|
|
{
|
|
frames[ 0 ].normal = new Vector3();
|
|
frames[ 0 ].binormal = new Vector3();
|
|
|
|
var min = float.MaxValue;
|
|
|
|
var tx = Math.Abs( frames[ 0 ].tangent.X );
|
|
var ty = Math.Abs( frames[ 0 ].tangent.Y );
|
|
var tz = Math.Abs( frames[ 0 ].tangent.Z );
|
|
|
|
var normal = Vector3.Zero;
|
|
|
|
if ( tx <= min )
|
|
{
|
|
min = tx;
|
|
normal = new Vector3( 1, 0, 0 );
|
|
|
|
}
|
|
|
|
if ( ty <= min )
|
|
{
|
|
min = ty;
|
|
normal = new Vector3( 0, 1, 0 );
|
|
|
|
}
|
|
|
|
if ( tz <= min )
|
|
{
|
|
normal = new Vector3( 0, 0, 1 );
|
|
}
|
|
|
|
var vec = frames[ 0 ].tangent.Cross( normal ).Normalized();
|
|
|
|
frames[ 0 ].normal = frames[ 0 ].tangent.Cross( vec );
|
|
frames[ 0 ].binormal = frames[ 0 ].tangent.Cross( frames[ 0 ].normal );
|
|
}
|
|
|
|
void _ComputeNormalsAndBinormals( List<FresnetFrame> frames, bool closed )
|
|
{
|
|
for ( int i = 1; i < frames.Count; i++ )
|
|
{
|
|
frames[ i ].normal = frames[ i - 1 ].normal;
|
|
frames[ i ].binormal = frames[ i - 1 ].binormal;
|
|
|
|
var vec = frames[ i - 1 ].tangent.Cross( frames[ i ].tangent );
|
|
|
|
if ( vec.Length() > Mathf.Epsilon )
|
|
{
|
|
vec = vec.Normalized();
|
|
|
|
var dot = frames[ i - 1 ].tangent.Dot( frames[ i ].tangent );
|
|
var angle = Mathf.Acos( Mathf.Clamp( dot, - 1, 1 ) );
|
|
|
|
var rotationMatrix = new Transform3D().Rotated( vec, angle );
|
|
frames[ i ].normal = frames[ i ].normal * rotationMatrix;
|
|
}
|
|
|
|
frames[ i ].binormal = frames[ i ].tangent.Cross( frames[ i ].normal );
|
|
|
|
}
|
|
|
|
if ( ! closed )
|
|
{
|
|
return;
|
|
}
|
|
|
|
|
|
var d = frames[ 0 ].normal.Dot( frames[ frames.Count - 1 ].normal );
|
|
|
|
var theta = Mathf.Acos( Mathf.Clamp( d, - 1, 1 ) );
|
|
theta /= frames.Count;
|
|
|
|
if ( frames[ 0 ].tangent.Dot( frames[ 0 ].normal.Cross( frames[ frames.Count - 1 ].normal ) ) > 0 ) {
|
|
|
|
theta = -theta;
|
|
}
|
|
|
|
for ( int i = 1; i < frames.Count; i ++ )
|
|
{
|
|
var rm = new Transform3D().Rotated( frames[ i ].tangent, theta * i );
|
|
frames[ i ].normal = frames[ i ].normal * rm;
|
|
frames[ i ].binormal = frames[ i ].tangent.Cross( frames[ i ].normal );
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
public virtual float ComputeSubLength( Range range, int numSamples )
|
|
{
|
|
var diff = range.length / ( numSamples - 1 ) ;
|
|
var it = PositionAt( range.min );
|
|
var length = 0f;
|
|
|
|
for ( var i = 1 ; i < numSamples; i++ )
|
|
{
|
|
var t = range.min + i * diff;
|
|
var next = PositionAt( t );
|
|
length += ( next - it ).Length();
|
|
it = next;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
public virtual int ComputeNumSamplesFor( Range range,
|
|
float minimumDistance, int minSamples = 2,
|
|
int numSamplesForLengthComputation = -1 )
|
|
{
|
|
if ( minSamples < 2 )
|
|
{ minSamples = 2; }
|
|
|
|
if ( numSamplesForLengthComputation <= 0 )
|
|
{ numSamplesForLengthComputation = minSamples * 4; }
|
|
|
|
float distance = ComputeSubLength( range, numSamplesForLengthComputation );
|
|
float numFloatingCuts = distance / minimumDistance;
|
|
int numSamples = Mathf.CeilToInt( numFloatingCuts );
|
|
|
|
return Mathf.Max( minSamples, numSamples );
|
|
}
|
|
|
|
public Vector3 GetClosestPositionTo( Vector3 position, int numSegments = 20, int depth = 0 )
|
|
{
|
|
var parameter = GetClosestParameterTo( position, numSegments, depth );
|
|
|
|
return PositionAt( parameter );
|
|
}
|
|
|
|
public float GetClosestParameterTo( Vector3 point, int numSegments = 20, int depth = 3 )
|
|
{
|
|
return GetClosestParameterTo( 0, 1, point, numSegments, depth, 0 );
|
|
}
|
|
|
|
public float GetDistanceTo( Vector3 point, int numSegments = 20, int depth = 3 )
|
|
{
|
|
var p = GetClosestPositionTo( point, numSegments, depth );
|
|
return ( p - point ).Length();
|
|
}
|
|
|
|
protected float GetClosestParameterTo( float tStart, float tEnd, Vector3 position, int numSegments, int maxDepth, int currentDepth = 0 )
|
|
{
|
|
var tNormalizer = ( tEnd - tStart ) / (float) ( numSegments - 1 );
|
|
|
|
var closestIndex = -1;
|
|
var closestDistance = float.MaxValue;
|
|
var minT = 0f;
|
|
var maxT = 1f;
|
|
var startPoint = PositionAt( tStart );
|
|
|
|
var line = new Line3();
|
|
var lastT = tStart;
|
|
|
|
for ( int i = 1; i < numSegments; i++ )
|
|
{
|
|
var t = tNormalizer * i + tStart;
|
|
var nextCurvePoint = PositionAt( t );
|
|
|
|
line.Set( startPoint, nextCurvePoint );
|
|
|
|
var closestPoint = line.ClosestPointToPoint( position );
|
|
|
|
var distance = ( position - closestPoint ).LengthSquared();
|
|
|
|
if ( distance < closestDistance )
|
|
{
|
|
closestDistance = distance;
|
|
closestIndex = i - 1;
|
|
minT = lastT;
|
|
maxT = t;
|
|
}
|
|
|
|
startPoint = nextCurvePoint;
|
|
lastT = t;
|
|
}
|
|
|
|
if ( maxDepth != currentDepth )
|
|
{
|
|
return GetClosestParameterTo( minT, maxT, position, numSegments, maxDepth, currentDepth + 1 );
|
|
}
|
|
|
|
var tNormalizer2 = ( maxT - minT ) / (float)(numSegments - 1 );
|
|
|
|
closestDistance = float.MaxValue;
|
|
var closestParameter = 0.5f;
|
|
|
|
for ( int i = 0; i < numSegments; i++ )
|
|
{
|
|
var detailedT = i * tNormalizer2 + minT;
|
|
var sampledPoint = PositionAt( detailedT );
|
|
|
|
var distance = ( sampledPoint - position ).LengthSquared();
|
|
|
|
if ( distance < closestDistance )
|
|
{
|
|
closestParameter = detailedT;
|
|
closestDistance = distance;
|
|
}
|
|
}
|
|
|
|
return closestParameter;
|
|
|
|
}
|
|
|
|
|
|
}
|
|
} |