diff --git a/src/lib.rs b/src/lib.rs
index 4e52076..095c647 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1502,10 +1502,10 @@ pub fn simple_compound_bca(x: f64, y: f64, z: f64, ux: f64, uy: f64, uz: f64, E1
 #[cfg(feature = "parry3d")]
 #[no_mangle]
 pub extern "C" fn rotate_given_surface_normal(nx: f64, ny: f64, nz: f64, ux: &mut f64, uy: &mut f64, uz: &mut f64) {
-    const DELTA: f64 = 1e-9;
-    let RUSTBCA_DIRECTION: Vector3::<f64> = Vector3::<f64>::new(1.0, 0.0, 0.0);
+    //const DELTA: f64 = 1e-9;
+    //let RUSTBCA_DIRECTION: Vector3::<f64> = Vector3::<f64>::new(1.0, 0.0, 0.0);
 
-    let into_surface = Vector3::new(-nx, -ny, -nz);
+    //let into_surface = Vector3::new(-nx, -ny, -nz);
     let direction = Vector3::new(*ux, *uy, *uz);
 
     //Rotation to local RustBCA coordinates from global
@@ -1514,12 +1514,15 @@ pub extern "C" fn rotate_given_surface_normal(nx: f64, ny: f64, nz: f64, ux: &mu
     //That rotation is then applied to the particle direction, and can be undone later.
     //Algorithm is from here:
     //https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/180436#180436
-    let v: Vector3<f64> = into_surface.cross(&RUSTBCA_DIRECTION);
-    let c = into_surface.dot(&RUSTBCA_DIRECTION);
-    let vx = Matrix3::<f64>::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0);
+    //let v: Vector3<f64> = into_surface.cross(&RUSTBCA_DIRECTION);
+    //let c = into_surface.dot(&RUSTBCA_DIRECTION);
+    //let vx = Matrix3::<f64>::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0);
+
+    //let s = ny*ny + nz*nz;
+
+    let rotation_matrix = if (1.0 - nx).abs() > 0.0 {
+        Matrix3::<f64>::new(1. + (-ny*ny - nz*nz)/(1. - nx), -ny, -nz, ny, -ny*ny/(1. - nx) + 1., -ny*nz/(1. - nx), nz, -ny*nz/(1. - nx), -nz*nz/(1. - nx) + 1.)
 
-    let rotation_matrix = if (c + 1.0).abs() > DELTA {
-        Matrix3::identity() + vx + vx*vx/(1. + c)
     } else {
         //If c == -1.0, the correct rotation should simply be a 180 degree rotation
         //around a non-x axis; y is chosen arbitrarily
@@ -1528,15 +1531,8 @@ pub extern "C" fn rotate_given_surface_normal(nx: f64, ny: f64, nz: f64, ux: &mu
 
     let incident = rotation_matrix*direction;
 
-    // ux must not be exactly 1.0 to avoid gimbal lock in RustBCA
-    // simple_bca does not normalize direction before proceeding, must be done manually
-    assert!(
-        incident.x + DELTA > 0.0, "Error: RustBCA initial direction out of surface. Please check surface normals and incident direction. c={} n = ({}, {}, {}) u = ({}, {}, {}), unew = ({}, {}, {})",
-        (c + 1.0).abs(), nx, ny, nz, ux, uy, uz, incident.x, incident.y, incident.z
-    );
-
-    *ux = incident.x + DELTA;
-    *uy = incident.y - DELTA;
+    *ux = incident.x;
+    *uy = incident.y;
     *uz = incident.z;
     let mag = (ux.powf(2.) + uy.powf(2.) + uz.powf(2.)).sqrt();
 
@@ -1615,7 +1611,7 @@ pub fn rotate_given_surface_normal_vec_py(nx: Vec<f64>, ny: Vec<f64>, nz: Vec<f6
 pub extern "C" fn rotate_back(nx: f64, ny: f64, nz: f64, ux: &mut f64, uy: &mut f64, uz: &mut f64) {
     let RUSTBCA_DIRECTION: Vector3::<f64> = Vector3::<f64>::new(1.0, 0.0, 0.0);
 
-    let into_surface = Vector3::new(-nx, -ny, -nz);
+    //let into_surface = Vector3::new(-nx, -ny, -nz);
     let direction = Vector3::new(*ux, *uy, *uz);
 
     //Rotation to local RustBCA coordinates from global
@@ -1623,12 +1619,8 @@ pub extern "C" fn rotate_back(nx: f64, ny: f64, nz: f64, ux: &mut f64, uy: &mut
     //into-the-surface vector (1.0, 0.0, 0.0) onto the local into-the-surface vector (negative normal w.r.t. ray origin).
     //That rotation is then applied to the particle direction, and can be undone later.
     //Algorithm is from here:
-    //https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/180436#180436
-    let v: Vector3<f64> = into_surface.cross(&RUSTBCA_DIRECTION);
-    let c = into_surface.dot(&RUSTBCA_DIRECTION);
-    let vx = Matrix3::<f64>::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0);
-    let rotation_matrix = if c != -1.0 {
-        Matrix3::identity() + vx + vx*vx/(1. + c)
+    let rotation_matrix = if (1.0 - nx).abs() > 0.0 {
+        Matrix3::<f64>::new(1. + (-ny*ny - nz*nz)/(1. - nx), -ny, -nz, ny, -ny*ny/(1. - nx) + 1., -ny*nz/(1. - nx), nz, -ny*nz/(1. - nx), -nz*nz/(1. - nx) + 1.)
     } else {
         //If c == -1.0, the correct rotation should simply be a 180 degree rotation
         //around a non-x axis; y is chosen arbitrarily