python - Generate trajectory along surface of a sphere with velocity - Stack Overflow

admin管理员组

文章数量:1302417

I am trying to generate trajectory between 2 points A and B along surface of a hemisphere.

Constraints:

• The end point of the trajectory(Point B) should have velocity in a certain direction given by (vx, vy, vz).

I have implemented simple SLERP(spherical linear interpolation) for this but how do I give velocity at point B?

Current Code:

# Simple SLERP

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def slerp(A, B, t):
    # Interpolates between points A and B on a sphere using Slerp.
    A = A / np.linalg.norm(A)  
    B = B / np.linalg.norm(B)

    dot_product = np.clip(np.dot(A, B), -1.0, 1.0)  # Clamped to avoid precision errors
    omega = np.arccos(dot_product)  # Angular distance

    if np.isclose(omega, 0):
        return A 

    return (np.sin((1 - t) * omega) * A + np.sin(t * omega) * B) / np.sin(omega)


A_cartesian = np.array([1, 0, 0])  # Point A on unit sphere
B_cartesian = np.array([0, 1, 0])  # Point B on unit sphere


num_points = 10  # Number of trajectory points
trajectory = np.array([slerp(A_cartesian, B_cartesian, t) for t in np.linspace(0, 1, num_points)])

# Visualization
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], 'bo-', label="Trajectory")
ax.scatter(*A_cartesian, color='red', label='Start (A)')
ax.scatter(*B_cartesian, color='green', label='End (B)')

# Sphere surface
u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:10j]
x = np.cos(u) * np.sin(v)
y = np.sin(u) * np.sin(v)
z = np.cos(v)
ax.plot_wireframe(x, y, z, color='gray', alpha=0.3)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.legend()
plt.show()

Let me know if there is any confusion regarding "What does velocity at point means".

I am trying to generate trajectory between 2 points A and B along surface of a hemisphere.

Constraints:

• The end point of the trajectory(Point B) should have velocity in a certain direction given by (vx, vy, vz).

I have implemented simple SLERP(spherical linear interpolation) for this but how do I give velocity at point B?

Current Code:

# Simple SLERP

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def slerp(A, B, t):
    # Interpolates between points A and B on a sphere using Slerp.
    A = A / np.linalg.norm(A)  
    B = B / np.linalg.norm(B)

    dot_product = np.clip(np.dot(A, B), -1.0, 1.0)  # Clamped to avoid precision errors
    omega = np.arccos(dot_product)  # Angular distance

    if np.isclose(omega, 0):
        return A 

    return (np.sin((1 - t) * omega) * A + np.sin(t * omega) * B) / np.sin(omega)


A_cartesian = np.array([1, 0, 0])  # Point A on unit sphere
B_cartesian = np.array([0, 1, 0])  # Point B on unit sphere


num_points = 10  # Number of trajectory points
trajectory = np.array([slerp(A_cartesian, B_cartesian, t) for t in np.linspace(0, 1, num_points)])

# Visualization
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], 'bo-', label="Trajectory")
ax.scatter(*A_cartesian, color='red', label='Start (A)')
ax.scatter(*B_cartesian, color='green', label='End (B)')

# Sphere surface
u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:10j]
x = np.cos(u) * np.sin(v)
y = np.sin(u) * np.sin(v)
z = np.cos(v)
ax.plot_wireframe(x, y, z, color='gray', alpha=0.3)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.legend()
plt.show()

Let me know if there is any confusion regarding "What does velocity at point means".

• python
• interpolation
• game-physics
• physics

Share Follow asked Feb 11 at 8:24 PrathamPratham 11577 bronze badges 3

• "Let me know if there is any confusion regarding "What does velocity at point means"." - well, I'm letting you know! – lastchance Commented Feb 11 at 9:13
• @lastchance Okay. Consider an example where the velocity at the end point needs to be x=5,y=0,z=0. In this case, the end point(Point B) should be facing in that direction(not just reaching Point B facing any direction). Also no sharp turns are allowed. means we cant reach Point B in from any direction and take a turn facing the desired direction. Note that the velocity of end point would be tangent to the surface of the hemisphere and the example of x=5, y=0, z=0 is definitely not tangent to the surface. its just an example. – Pratham Commented Feb 12 at 14:50
• So all you actually want is that the final direction is tangential to the surface. Doesn't Neil Butcher's solution (with the anaytical derivative) do that? If so then you should accept his answer. – lastchance Commented Feb 12 at 15:38

Add a comment  | 

1 Answer 1

Sorted by: Reset to default 0

You're already plotting a path in space with a parameter you've named t. The direction of the velocity is the derivative of the path with respect to this parameter, t (the scale of the velocity is not meaningful unless we introduce some times).

You can take this derivative either analytically or numerically. Here is example code which takes the numerical derivative lazily (in a function I've called slerp_dot ) if you have the time then you can find the maths to replace the numerical derivative.

# Simple SLERP

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def slerp(A, B, t):
    # Interpolates between points A and B on a sphere using Slerp.
    A = A / np.linalg.norm(A)
    B = B / np.linalg.norm(B)

    dot_product = np.clip(np.dot(A, B), -1.0, 1.0)  # Clamped to avoid precision errors
    omega = np.arccos(dot_product)  # Angular distance

    if np.isclose(omega, 0):
        return A

    return (np.sin((1 - t) * omega) * A + np.sin(t * omega) * B) / np.sin(omega)


def slerp_dot(A, B, t):
    delta = 0.01

    return (slerp(A, B, t + delta)-slerp(A, B, t - delta))/2.0/delta


A_cartesian = np.array([1, 0, 0])  # Point A on unit sphere
B_cartesian = np.array([0, 1, 0])  # Point B on unit sphere


num_points = 10  # Number of trajectory points
trajectory = np.array([slerp(A_cartesian, B_cartesian, t) for t in np.linspace(0, 1, num_points)])

velocity = slerp_dot(A_cartesian, B_cartesian, 1.0)

# Visualization
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], 'bo-', label="Trajectory")
ax.scatter(*A_cartesian, color='red', label='Start (A)')
ax.scatter(*B_cartesian, color='green', label='End (B)')

# Sphere surface
u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:10j]
x = np.cos(u) * np.sin(v)
y = np.sin(u) * np.sin(v)
z = np.cos(v)
ax.plot_wireframe(x, y, z, color='gray', alpha=0.3)

# Show the velocity
ax.quiver(B_cartesian[0], B_cartesian[1], B_cartesian[2], velocity[0], velocity[1], velocity[2], length=0.5)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.legend()
plt.show()

EDIT: It was really small so here is the analytical derivative if you prefer that

def slerp_dot(A, B, t):
    A = A / np.linalg.norm(A)
    B = B / np.linalg.norm(B)

    dot_product = np.clip(np.dot(A, B), -1.0, 1.0)  # Clamped to avoid precision errors
    omega = np.arccos(dot_product)  # Angular distance
    return (-omega * np.cos((1 - t) * omega) * A + omega * np.cos(t * omega) * B) / np.sin(omega)

本文标签： pythonGenerate trajectory along surface of a sphere with velocityStack Overflow