Reputation: 3
I have a function that places a trimesh primitive cylinder (pin) on a defined XYZ coordinate (x_cylinder_points) so that it rests on another cylinder (cylinder), and then it would ideally orient the pin so that it points towards a second defined XYZ coordinate (x_connection_points).
As a result, I get pins that point away from the second point and not at an angle.
I'm not too sure what is causing the issue, I suspect that it is the values I am using for my direction but I do not know why It is incorrect.
here is my implementation:
EDIT:
import trimesh
import math
import numpy as np
#input two XYZ points, return distance between points
def distance_between_points(point1, point2):
# Convert the points to numpy arrays.
point1 = np.array(point1)
point2 = np.array(point2)
# Calculate the difference between the two points.
difference = np.subtract(point2, point1)
# Calculate the norm of the difference vector.
norm = np.linalg.norm(difference)
# Return the distance.
return norm
def find_angle_between_points(point1, point2):
# Calculate the angle between the two vectors
print("P1 ", point1)
print("P2 ", point2)
angle = np.arccos(np.dot(point1, point2-point1) / (np.linalg.norm(point1) * np.linalg.norm(point2-point1)))
print("NormP1 ", np.linalg.norm(point1))
print("NormP2 ", np.linalg.norm(point2))
return np.degrees(angle)
def read_blue_points_file(filename):
"""Reads a text file of blue points and returns an array of points."""
points = []
with open(filename, "r") as f:
for line in f:
point = [float(x) for x in line.split(",")]
points.append(point)
return np.array(points)
def find_greatest_value_index(list_of_numbers):
"""Finds the index of the greatest value in a list."""
greatest_value_index = 0
greatest_value = list_of_numbers[0]
for index, number in enumerate(list_of_numbers):
if number > greatest_value:
greatest_value_index = index
greatest_value = number
return greatest_value_index
def translate_points(points, difference, height):
translated_points = []
for point in points:
x, y, z = point
x_sign = math.copysign(1, x)
translated_points.append([x + x_sign * difference, y - height, z])
return translated_points
def main():
print("IN MAIN")
connection_points = read_blue_points_file("blue_points.txt") # original points along JAW
cylinder_points = translate_points(connection_points, 6, 4) # translated points where cylinder is placed
cylinder_points = np.array(cylinder_points)
cylinders = []
pins = []
scene = trimesh.Scene()
# Sort the array by the X coordinate
x_cylinder_points = cylinder_points[cylinder_points[:, 0].argsort()]
x_connection_points = connection_points[connection_points[:, 0].argsort()]
print(cylinder_points)
collision_manager_jaw = trimesh.collision.CollisionManager()
collision_manager_jaw.add_object("jaw", jaw)
#place cylinders
for index, point in reversed(list(enumerate(x_cylinder_points))):
height = point[2]
cylinder = trimesh.primitives.Cylinder(radius=0.5, height=height, transform=None, sections=32, mutable=True)
cylinder.apply_translation((point[0], point[1], height / 2))
cylinders.append(cylinder)
# place pin heads
for (cylinder_point, connection_point) in zip(x_cylinder_points, x_connection_points):
current_cylinder = cylinder_point
current_connect = connection_point
Angle = find_angle_between_points(current_cylinder, current_connect)
Direction = current_connect - current_cylinder
print("direction", Direction)
print("angle ", Angle)
rotation_matrix = trimesh.geometry.align_vectors([0, 0, 1], Direction, return_angle=False)
print("r matrix ", rotation_matrix)
distance = distance_between_points(current_cylinder, current_connect)
print(distance)
pin = trimesh.primitives.Cylinder(radius=0.35, height=distance, transform=rotation_matrix, sections=32, mutable=True)
pin.apply_translation(current_cylinder - Direction * 0.5)
pins.append(pin)
scene.add_geometry(pins)
scene.add_geometry(cylinders)
scene.show()
if __name__ == "__main__":
main()
The text file for the blue_points can be found here text File
The download to this code can be found here Code
Thank You : )
Upvotes: 0
Views: 43