rj-action-library/Runtime/Math/Geometry/Pose.cs

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

namespace Rokojori
{
  public class Pose
	{
    bool _needsUpdate = true;
    Quaternion _rotation = Quaternion.Identity;
    public Quaternion rotation 
    {
      get => _rotation;
      set
      {
        _rotation = value;
        _needsUpdate = true;
      }
    }

    Vector3 _position = Vector3.Zero;
    public Vector3 position
    {
      get => _position;
      set
      {
        _position = value;
        _needsUpdate = true;
      }    
      
    }

    Basis _basis;

    void Update()
    {
      if ( ! _needsUpdate )
      {
        return;
      }
      
      _needsUpdate = false;

      _basis = new Basis( _rotation );
    }

    public Vector3 forward
    {
      get
      {
        Update();

        return -_basis.Z; 
      }
    }

    public Vector3 right
    {
      get
      {
        Update();

        return _basis.X; 
      }
    }

    public Vector3 up
    {
      get
      {
        Update();

        return _basis.Y; 
      }
    }

    public Pose()
    {

    }


    public Pose ToGlobal( Node3D n )
    {
      var p = new Pose();
      p.rotation = rotation * n.GetGlobalQuaternion();
      p.position = n.ToGlobal( position );
      return p;
    }

    public Vector3 Apply( Vector3 p )
    {
      p = rotation * p;
      p += position;

      return p;
    }

  }
}