Coordinates

def cylindrical_from_cartesian(p):
    """
    Convert Cartesian coordinates to cylindrical coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Cartesian coordinates (x, y, z)
    
    Returns:
    - q: array, shape (..., 3) - Cylindrical coordinates (rho, phi, z)
    """

def spherical_from_cartesian(p):
    """
    Convert Cartesian coordinates to spherical coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Cartesian coordinates (x, y, z)
    
    Returns:
    - q: array, shape (..., 3) - Spherical coordinates (rho, theta, phi)
    """

def cartesian_from_cylindrical(p):
    """
    Convert cylindrical coordinates to Cartesian coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Cylindrical coordinates (rho, phi, z)
        - rho: radial distance from z-axis
        - phi: azimuth angle in radians
        - z: height along z-axis
    
    Returns:
    - q: array, shape (..., 3) - Cartesian coordinates (x, y, z)
    """

def spherical_from_cylindrical(p):
    """
    Convert cylindrical coordinates to spherical coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Cylindrical coordinates (rho, phi, z)
    
    Returns:
    - q: array, shape (..., 3) - Spherical coordinates (rho, theta, phi)
    """

def cartesian_from_spherical(p):
    """
    Convert spherical coordinates to Cartesian coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Spherical coordinates (rho, theta, phi)
        - rho: radial distance from origin
        - theta: polar angle from positive z-axis (0 to π)
        - phi: azimuth angle in x-y plane from positive x-axis
    
    Returns:
    - q: array, shape (..., 3) - Cartesian coordinates (x, y, z)
    """

def cylindrical_from_spherical(p):
    """
    Convert spherical coordinates to cylindrical coordinates.
    
    Parameters:
    - p: array-like, shape (..., 3) - Spherical coordinates (rho, theta, phi)
    
    Returns:
    - q: array, shape (..., 3) - Cylindrical coordinates (rho, phi, z)
    """

import numpy as np
import pytransform3d.coordinates as pco

# Define points in Cartesian coordinates
cartesian_points = np.array([
    [1, 0, 0],      # x-axis
    [0, 1, 0],      # y-axis  
    [0, 0, 1],      # z-axis
    [1, 1, 1],      # diagonal
])

print("Original Cartesian coordinates:")
print(cartesian_points)

# Convert to cylindrical
cylindrical = pco.cylindrical_from_cartesian(cartesian_points)
print("\nCylindrical coordinates (rho, phi, z):")
print(cylindrical)

# Convert to spherical
spherical = pco.spherical_from_cartesian(cartesian_points)
print("\nSpherical coordinates (rho, theta, phi):")
print(spherical)

# Verify round-trip conversion
cartesian_recovered = pco.cartesian_from_spherical(spherical)
print("\nRecovered Cartesian coordinates:")
print(cartesian_recovered)
print(f"Conversion error: {np.max(np.abs(cartesian_points - cartesian_recovered))}")

import numpy as np
import pytransform3d.coordinates as pco

# Create grid of points
x = np.linspace(-2, 2, 5)
y = np.linspace(-2, 2, 5) 
z = np.linspace(0, 2, 3)

# Create meshgrid
X, Y, Z = np.meshgrid(x, y, z)
cartesian_grid = np.stack([X.flatten(), Y.flatten(), Z.flatten()], axis=1)

print(f"Grid shape: {cartesian_grid.shape}")

# Convert entire grid to cylindrical coordinates
cylindrical_grid = pco.cylindrical_from_cartesian(cartesian_grid)

# Extract components
rho = cylindrical_grid[:, 0]
phi = cylindrical_grid[:, 1] 
z_cyl = cylindrical_grid[:, 2]

print(f"Radial distances range: {rho.min():.3f} to {rho.max():.3f}")
print(f"Azimuth angles range: {phi.min():.3f} to {phi.max():.3f} radians")
print(f"Heights range: {z_cyl.min():.3f} to {z_cyl.max():.3f}")

import numpy as np
import pytransform3d.coordinates as pco

# Define point in cylindrical coordinates
cylindrical_point = np.array([2.0, np.pi/4, 1.0])  # rho=2, phi=45°, z=1
print(f"Cylindrical: rho={cylindrical_point[0]}, phi={cylindrical_point[1]:.3f}, z={cylindrical_point[2]}")

# Convert cylindrical -> cartesian -> spherical
cartesian = pco.cartesian_from_cylindrical(cylindrical_point)
spherical = pco.spherical_from_cartesian(cartesian)

print(f"Cartesian: x={cartesian[0]:.3f}, y={cartesian[1]:.3f}, z={cartesian[2]:.3f}")
print(f"Spherical: rho={spherical[0]:.3f}, theta={spherical[1]:.3f}, phi={spherical[2]:.3f}")

# Direct cylindrical -> spherical conversion
spherical_direct = pco.spherical_from_cylindrical(cylindrical_point)
print(f"Direct conversion: rho={spherical_direct[0]:.3f}, theta={spherical_direct[1]:.3f}, phi={spherical_direct[2]:.3f}")

# Verify both paths give same result
print(f"Conversion difference: {np.max(np.abs(spherical - spherical_direct))}")

import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.coordinates as pco

# Generate points on sphere in spherical coordinates
n_points = 1000
rho = 1.0  # unit sphere
theta = np.random.uniform(0, np.pi, n_points)  # polar angle
phi = np.random.uniform(0, 2*np.pi, n_points)  # azimuth angle

spherical_points = np.column_stack([np.full(n_points, rho), theta, phi])

# Convert to Cartesian for plotting  
cartesian_points = pco.cartesian_from_spherical(spherical_points)

# Plot 3D scatter
fig = plt.figure(figsize=(12, 4))

# Cartesian view
ax1 = fig.add_subplot(131, projection='3d')
ax1.scatter(cartesian_points[:, 0], cartesian_points[:, 1], cartesian_points[:, 2], 
           c=theta, cmap='viridis', s=1)
ax1.set_title('Cartesian View')
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')

# Cylindrical projection (top view)
cylindrical_points = pco.cylindrical_from_cartesian(cartesian_points)
ax2 = fig.add_subplot(132)
ax2.scatter(cylindrical_points[:, 0] * np.cos(cylindrical_points[:, 1]), 
           cylindrical_points[:, 0] * np.sin(cylindrical_points[:, 1]),
           c=cylindrical_points[:, 2], cmap='plasma', s=1)
ax2.set_title('Cylindrical Projection')
ax2.set_xlabel('Rho * cos(phi)')
ax2.set_ylabel('Rho * sin(phi)')
ax2.set_aspect('equal')

# Spherical coordinates plot
ax3 = fig.add_subplot(133)
ax3.scatter(phi, theta, c=rho, s=1)
ax3.set_title('Spherical Coordinates')
ax3.set_xlabel('Phi (azimuth)')
ax3.set_ylabel('Theta (polar)')

plt.tight_layout()
plt.show()