Fixed the vector math and added cross product