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;
      }
    }

    public void ApplyTwist( float twist )
    {
      rotation = rotation * Math3D.RotateZ( twist );
    }

    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; 
      }
    }


    Vector3? explicitUp = null;
    public Vector3 up
    {
      get
      {
        if ( explicitUp != null )
        {
          return (Vector3) explicitUp;
        }

        Update();

        return _basis.Y; 
      }

      set
      {
        explicitUp = value;
      }
    }

    public Pose()
    {

    }

    public static Pose Create( Vector3 position, Quaternion rotation )
    {
      var p = new Pose();
      p.position = position;
      p.rotation = rotation;
      return p;
    }

    public static Pose From( Pose a )
    {
       var p = new Pose();
      p.position = a.position;
      p.rotation = a.rotation;
      return p;

    }

    public static Pose Lerp( Pose a, Pose b, float weight = 0.5f )
    {
      var p = From( a );
      p.position = p.position.Lerp( b.position, weight );
      p.rotation = p.rotation.Slerp( b.rotation, weight );
      return p;

    }

    public void LookAt( Vector3 forward, Vector3 up )
    {
      rotation = Math3D.LookRotation( forward, up );
    }

    public static Pose From( Node3D node3D )
    {
      var p = new Pose();
      p.position = node3D.GlobalPosition;
      p.rotation = node3D.GetGlobalQuaternion();
      return p;
    }

    public static Pose InverseFrom( Node3D node3D )
    {
      var p = new Pose();
      p.position = - node3D.GlobalPosition;
      p.rotation = node3D.GetGlobalQuaternion().Inverse();
      return p;
    }

    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;
    }

    public override string ToString()
    {
      return "Pose{" + RJLog.Stringify( position ) + RJLog.Stringify( rotation ) + "}";
    } 

    public Vector3 ApplyInverse( Vector3 p )
    {
      p -= position;
      p = rotation.Inverse() * p;     

      return p;
    }

    public Vector3 OnRightUpCircle( float radians, float size = 1 )
    {
      var c = Mathf.Cos( radians ) * size;
      var s = Mathf.Sin( radians ) * size;

      return s * right + c * up + position;
    }

    public Vector3 ComputeCircleValues( float radians, Vector3 cos, Vector3 sin, float size )
    {
      var c = Mathf.Cos( radians ) * size;
      var s = Mathf.Sin( radians ) * size;

      var cValue = c * cos.X * right +
                   c * cos.Y * up +
                   c * cos.Z * forward;

      var sValue = s * sin.X * right +
                   s * sin.Y * up +
                   s * sin.Z * forward;

      return cValue + sValue + position;
    }


    public static Pose Merge( List<Pose> poses, List<float> weights = null )
    {
      var position  = Vector3.Zero;
      var up        = Vector3.Zero;
      var forward   = Vector3.Zero;

      if ( weights == null )
      {
        weights = new List<float>( poses.Count );

        for ( int i= 0; i < poses.Count; i++ )
        {
          weights[ i ] = 1f / poses.Count;
        }
      }

      for ( int i = 0; i < poses.Count; i++ )
      {
        position += poses[ i ].position * weights[ i ];
        up       += poses[ i ].up * weights[ i ];
        forward  += poses[ i ].forward * weights[ i ];   
      }

      return Create( position, Math3D.LookRotation( forward, up ) );
    }

  }
}