added vector1
cleanups and added const where applicable
angle is a vector of N dimensions, where N is the number of degree of freedom