bug on math::angleAxisToQuaternion

[dytan]

Hello, found a bug on math.cpp angleAxisToQuaternion:

inline void angleAxisToQuaternion(const Vec<3> &a1, const double theta, Vec<4>& quaternion) {
  quaternion[0] = cos(theta * .5);
  quaternion[0] = a1[0] * sin(theta * .5);
  quaternion[1] = a1[1] * sin(theta * .5);
  quaternion[2] = a1[2] * sin(theta * .5);
}

index is not right, suppose to be:

inline void angleAxisToQuaternion(const Vec<3> &a1, const double theta, Vec<4>& quaternion) {
  quaternion[0] = cos(theta * .5);
  quaternion[1] = a1[0] * sin(theta * .5);
  quaternion[2] = a1[1] * sin(theta * .5);
  quaternion[3] = a1[2] * sin(theta * .5);
}

[jhwangbo]

Thank you for reporting a bug. It is fixed in the latest commit.