//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
vtx::vertex3<T> rotate(const vtx::vertex3<T>& vec) const {
vtx::vertex3<T> vn;
vtx::normalize<T>(vec, vn);
quaternion vecQ(vn.x, vn.y, vn.z, 0);
quaternion resQ = vecQ * get_conjugate();
resQ = *this * resQ;
vtx::vertex3<T> out(resQ.x, resQ.y, resQ.z);
return std::move(out);
}
void world_to_local(struct robot * robot, struct vect *local,
struct vect *world)
{
struct quaternion backwards;
struct vect temp = *world;
temp.x -= robot->position2.position.x;
temp.y -= robot->position2.position.y;
get_conjugate(&backwards, &robot->position2.corrected_orientation);
rotate_vector(local, &temp, &backwards);
}
void rotate_vector(struct vect *out, struct vect *in, struct quaternion *r)
{
struct quaternion temp, sum, res, inverse;
temp.x = in->x;
temp.y = in->y;
temp.z = in->z;
temp.w = 0;
get_conjugate(&inverse, r);
quaternion_product(&sum, r, &temp);
quaternion_product(&res, &sum, &inverse);
out->x = res.x;
out->y = res.y;
out->z = res.z;
}
