rj-action-library/Runtime/Interactions/MultiRayCaster.cs

using Godot;
using System.Collections;
using System.Collections.Generic;
using Godot.Collections;

namespace Rokojori
{

  [GlobalClass]
  public partial class MultiRayCaster:RJCaster
  {
    [Export]
    public float rayLength = 10;

    [Export]
    public int maxHits = 100;

    [Export]
    public bool sortByPointerPriority;

    List<CollisionData> collisions = new List<CollisionData>();
    int numCollisions = 0;

    [Export]
    public Vector3 to;

    public override int NumColliders()
    {
      return numCollisions;
    }

    
    public override Node GetCollider( int index )
    {
      return collisions[ index ].collider;
    }

    public override Vector3 GetCollisionNormal( int index )
    {
      return collisions[ index ].normal;
    }

    public override Vector3 GetCollisionPosition( int index )
    {
      return collisions[ index ].position;
    }

    public override Shape3D GetCollisionShape( int index )
    {
      return collisions[ index ].shape;
    }

    PhysicsRayQueryParameters3D rayParameters = new PhysicsRayQueryParameters3D();

    public override void _Process( double delta )
    {
      Actions.Trigger( BeforeProcess );

      ResolveCollisions();
      SortCollisions(); 


      Actions.Trigger( AfterProcess );

    } 

    ValueSorter<CollisionData> singleSorter;
    MultiValueSorter<CollisionData> multiSorter;
    
    

    CollisionData GetCollisionData( int i )
    {
      while ( collisions.Count <= i )
      {
        collisions.Add( new CollisionData() );
      }

      return collisions[ i ];
    }

    void ResolveCollisions()
    {
      var physics = GetWorld3D().DirectSpaceState;

      var excludes = new Array<Rid>();

      var from = GlobalPosition;
      var to   = from + Math3D.GetGlobalForward( this ) * rayLength;

      this.to = to;

      rayParameters.From = from;
      rayParameters.To = to;

      numCollisions = 0;

      for ( int i = 0; i < maxHits; i++ )
      {        
        rayParameters.Exclude = excludes;

        var collisionData = GetCollisionData( numCollisions );
        collisionData.Get( rayParameters, physics );

        if ( ! collisionData.hasCollision )
        {
          return;
        }
        
        excludes.Add( collisionData.rid );

        if ( IsSelected( collisionData ) )
        {          
          numCollisions ++;
        }

      }
    }

    bool IsSelected( CollisionData cd )
    {
      var includeSelector = IncludeSelector;
      var excludeSelector = ExcludeSelector;

      if ( includeSelector != null && ! includeSelector.Selects( cd.collider ) )
      {
        return false;
      }

      if ( excludeSelector != null && excludeSelector.Selects( cd.collider ) )
      {
        return false;
      }

      return true;
    }

    void SortCollisions()
    {
      if ( ! sortByPointerPriority )
      {
        if ( singleSorter == null )
        {
          singleSorter = ValueSorter<CollisionData>.Create( cd => GetDistance( cd ) );
        }

        singleSorter.Sort( 0, numCollisions, collisions );
      }
      else 
      {
        if ( multiSorter == null )
        {
          multiSorter = MultiValueSorter<CollisionData>.Create( 
            cd => GetPointablePriority( cd ),
            cd => GetDistance( cd )
          );
        }

        multiSorter.Sort( 0, numCollisions, collisions );
      }
    }

    float GetPointablePriority( CollisionData cd )
    {
      float priority = 0;

      var pointable = Nodes.Find<RJPointable>( cd.collider );

      if ( pointable != null )
      {
        priority = pointable.PointingPriority;
      }

      return priority; 
    }

    float GetDistance( CollisionData cd )
    {
      return ( cd.position - GlobalPosition ).Length(); 
    }


  }
}