R/tMatrixEP.R

In aaronolsen/linkR: 3D Lever and Linkage Mechanism Modeling

tMatrixEP <- function(v, a){
	# Top version from https://en.wikipedia.org/wiki/Rotation_matrix
	# Bottom from ?
	# Both give the same result

	# If zero length axis, return identity matrix
	#if(sum(abs(a)) < 1e-12) return(diag(3))

	if(length(v) == 1) stop('"v" must be a vector of length 3')
	if(length(a) > 1) stop('"a" must be a single value (angle)')

	# For some reason the resulting rotation matrix does not follow the right-hand rule... Flip angle so that it is right-hand
	a <- -a

	v <- uvector(v)

	r <- matrix(0, 3, 3)
	r[1, ] <- c(cos(a)+v[1]^2*(1-cos(a)), v[1]*v[2]*(1-cos(a))-v[3]*sin(a), v[1]*v[3]*(1-cos(a))+v[2]*sin(a))
	r[2, ] <- c(v[2]*v[1]*(1-cos(a))+v[3]*sin(a), cos(a) + v[2]^2*(1-cos(a)), v[2]*v[3]*(1-cos(a))-v[1]*sin(a))
	r[3, ] <- c(v[3]*v[1]*(1-cos(a))-v[2]*sin(a), v[3]*v[2]*(1-cos(a))+v[1]*sin(a), cos(a)+v[3]^2*(1-cos(a)))
	
	return(r)
}