screwed

screwed.rs

use nalgebra::{Vector3, Vector6, Unit};

// Converts an axis-angle representation to a twist
fn axis_angle_to_twist(axis: Unit<Vector3<f64>>, angle: f64) -> Vector6<f64> {
    let angular_velocity = angle * axis;
    Vector6::new(0.0, 0.0, 0.0, angular_velocity.x, angular_velocity.y, angular_velocity.z)
}

// Rotates a point using a twist
fn rotate_point(point: Vector3<f64>, twist: Vector6<f64>, angle: f64) -> Vector3<f64> {
    // Extract the angular velocity from the twist
    let angular_velocity = twist.fixed_slice::<U3, U1>(3, 0);
    
    // Calculate the rotation matrix from the angular velocity
    let rotation_axis = Unit::new_normalize(angular_velocity);
    let rotation_angle = angular_velocity.norm() * angle;
    let rotation_matrix = nalgebra::Rotation3::from_axis_angle(&rotation_axis, rotation_angle).matrix();

    // Rotate the point using the rotation matrix
    rotation_matrix * point
}

// Converts a 3D rotation and translation into a twist
fn pose_to_twist(axis: Unit<Vector3<f64>>, angle: f64, translation: Vector3<f64>) -> Vector6<f64> {
    let angular_velocity = angle * axis;
    let linear_velocity = translation / angle;
    Vector6::new(linear_velocity.x, linear_velocity.y, linear_velocity.z, angular_velocity.x, angular_velocity.y, angular_velocity.z)
}

// Transforms a point using a twist
fn transform_point(point: Vector3<f64>, twist: Vector6<f64>, angle: f64) -> Vector3<f64> {
    // Extract the linear and angular velocities from the twist
    let linear_velocity = twist.fixed_slice::<U3, U1>(0, 0);
    let angular_velocity = twist.fixed_slice::<U3, U1>(3, 0);

    // Calculate the rotation matrix from the angular velocity
    let rotation_axis = Unit::new_normalize(angular_velocity);
    let rotation_angle = angular_velocity.norm() * angle;
    let rotation_matrix = nalgebra::Rotation3::from_axis_angle(&rotation_axis, rotation_angle).matrix();
    // Calculate the translation vector from the linear velocity
    let translation_vector = linear_velocity * angle;

    // Transform the point using the rotation matrix and translation vector
    (rotation_matrix * point) + translation_vector
}

fn main() {
    // Define a rotation axis, angle, and translation
    let axis = Vector3::new(1.0, 0.0, 0.0);
    let angle = std::f64::consts::PI / 4.0; // 45 degrees
    let translation = Vector3::new(2.0, 0.0, 0.0);

    // Convert the rotation and translation to a unit axis and a twist
    let unit_axis = Unit::new_normalize(axis);
    let twist = pose_to_twist(unit_axis, angle, translation);

    // Define a point to be transformed
    let point = Vector3::new(1.0, 1.0, 1.0);

    // Transform the point using the twist
    let transformed_point = transform_point(point, twist, 1.0);

    println!("Original point: {:?}", point);
    println!("Transformed point: {:?}", transformed_point);
}