GIo
GIo

Reputation: 3

How can I make this rotational matrix work correctly?

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:

  1. get a list of predefined points called connection points
  2. create a second list of translated points (from connection points) called cylinder points
  3. Place upright base cylinders (place cylinders) using cylinder points
  4. Placing pins is the section where I am having issue, I take both of the lists named connection points and cylinder points and I put them both into one for loop. I then would make a solid starting from a point in cylinder points, and then points towards a point in connection points. Instead of that, My result does not angle towards the point, and it points in the opposite direction away from the intended direction.
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 : )

Image of Current Result

Upvotes: 0

Views: 43

Answers (0)

Related Questions