rokojori_action_library/Runtime/Physics/CollisionData.cs

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

namespace Rokojori
{
  public class CollisionData
  {
    public bool hasCollision = false;
    public Node collider;
    public Vector3 normal;
    public Vector3 position;
    public Shape3D shape;
    public Rid rid;


    public void CopyFrom( KinematicCollision3D kinematicCollision3D, int collisionIndex = 0 )
    {
      if ( kinematicCollision3D.GetCollisionCount() <= collisionIndex )
      {
        Reset();
        return;
      }

      hasCollision = true;
      position = kinematicCollision3D.GetPosition();
      normal = kinematicCollision3D.GetNormal();
      collider = (Node) kinematicCollision3D.GetCollider( collisionIndex );
      rid = (Rid) kinematicCollision3D.GetColliderRid( collisionIndex );
      shape = (Shape3D) kinematicCollision3D.GetColliderShape( collisionIndex );
    }

    void Reset()
    {
      hasCollision = false;

      position = Vector3.Zero;
      normal = Vector3.Up;
      collider = null;
      rid = new Rid();
      shape = null;
    }

    public void CopyFrom( CollisionData collisionData )
    {
      if ( ! collisionData.hasCollision )
      {
        Reset();
        return;
      }

      hasCollision = true;
      position = collisionData.position;
      normal = collisionData.normal;
      collider = collisionData.collider;
      rid = collisionData.rid;
      shape = collisionData.shape;
    }

    public static List<CollisionData> ForMaxHits( int maxHits )
    {
      var data = new List<CollisionData>();

      for ( int i = 0; i < maxHits; i++ )
      {
        data.Add( new CollisionData() );
      }

      return data;
    }

    public static bool HasCollisionMask( Node n )
    {
      return n is CsgShape3D || n is CollisionObject3D;
    }

    public static uint GetCollisionMask( Node n )
    {
      if ( n is CsgShape3D )
      {
        return ( n as CsgShape3D ).CollisionMask;
      }

      if ( n is CollisionObject3D )
      {
        return ( n as CollisionObject3D ).CollisionMask;
      }

      return 0;
    }

    public static bool HasCollisionLayer( Node n )
    {
      return n is CsgShape3D || n is CollisionObject3D;
    }

    public static uint GetCollisionLayer( Node n )
    {
      if ( n is CsgShape3D )
      {
        return ( n as CsgShape3D ).CollisionLayer;
      }

      if ( n is CollisionObject3D )
      {
        return ( n as CollisionObject3D ).CollisionLayer;
      }

      return 0;
    }


    public void Get( PhysicsRayQueryParameters3D ray, PhysicsDirectSpaceState3D physicsState )
    {
      var result = physicsState.IntersectRay( ray );

      if ( ! result.ContainsKey( "collider" ) || result[ "collider" ].As<Node>() == null )
      {
        hasCollision = false;
        return;
      }

      hasCollision = true;

      

      collider = result[ "collider" ].As<Node>();
      normal   = result[ "normal" ].AsVector3();
      position = result[ "position" ].AsVector3();
      shape    = result[ "shape" ].As<Shape3D>();
      rid      = result[ "rid" ].AsRid();

      //  RJLog.Log( "Has Collision:", HierarchyName.Of( collider ), ">> at position:", position, "with normal:", normal  );
 
    }

    public static CollisionData FindCollision( World3D world, PhysicsRayQueryParameters3D rayParameters, Func<CollisionData,bool> predicate )
    {
      var physics = world.DirectSpaceState;

      var excludes = rayParameters.Exclude;

      var maxHits = 10000;

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

        var collisionData = new CollisionData();
        collisionData.Get( rayParameters, physics );

        if ( ! collisionData.hasCollision )
        {
          return null;
        }

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

      }

      return null;
    
    }

    public static int MultiRayCast( World3D world, PhysicsRayQueryParameters3D rayParameters, List<CollisionData> collisionsOutput )
    {
      var physics = world.DirectSpaceState;

      var excludes = rayParameters.Exclude;

      var numCollisions = 0;

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

        var collisionData = collisionsOutput[ i ];
        collisionData.Get( rayParameters, physics );

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

        numCollisions ++;
      }

      return numCollisions;
    
    }

    public static List<CollisionData> MultiRayCast( World3D world, PhysicsRayQueryParameters3D rayParameters, int maxHits = 100 )
    {
      var physics = world.DirectSpaceState;

      var excludes = rayParameters.Exclude;

      var collisions = new List<CollisionData>();

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

        var collisionData = new CollisionData();
        collisionData.Get( rayParameters, physics );

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

        collisions.Add( collisionData );
      }

      return collisions;
    
    }
  }
}