def tras_rot2(beam, p, theta):
theta_grazing = np.pi / 2 - theta
y = Vector(0., 1., 0.)
y.rotation(-theta_grazing, 'x')
x = Vector(1., 0., 0.)
xp = x.vector_product(y)
vrot = y.rodrigues_formula(xp, -theta_grazing)
vrot.normalization()
position = Vector(beam.x, beam.y, beam.z)
velocity = Vector(beam.vx, beam.vy, beam.vz)
position.rotation(-theta_grazing, 'x')
velocity.rotation(-theta_grazing, 'x')
#position = position.rodrigues_formula(xp, -theta_grazing)
#velocity = velocity.rodrigues_formula(xp, -theta_grazing)
velocity.normalization()
beam.vx = velocity.x
beam.vy = velocity.y
beam.vz = velocity.z
vector_point = Vector(0, p, 0)
vector_point.rotation(-theta_grazing, "x")
print(vector_point.info())
#vector_point = vector_point.rodrigues_formula(xp, -theta_grazing)
vector_point.normalization()
beam.x = position.x - vector_point.x * p
beam.y = position.y - vector_point.y * p
beam.z = position.z - vector_point.z * p
def test_rodrigues(self):
print(">> test_rodrigues")
theta=90*np.pi/180
axis=Vector(0,0,1)
v=Vector(0,1,0)
vrot=v.rodrigues_formula(axis,theta)
print(v.info())
assert_almost_equal(vrot.x, -1.0, 15)
assert_almost_equal(vrot.y, 0, 15)
assert_almost_equal(vrot.z, 0, 15)
