void Vec3D::rotate(const Vec3D& axis)
{
double angle = axis.norm();
if(angle == 0)return;
Vec3D e_axis(axis/angle);
double par = (*this)*e_axis;
Vec3D p_axis(*this - par*e_axis);
// "Rotation Formula" -- see Goldstein chap 4
*this = par*e_axis + cos(angle)*p_axis + sin(angle)*(e_axis^p_axis);
}
Vec3D& Vec3D::operator &= (const Vec3D& r)
{
#define R1 (*this)
#define R2 r
const double r1_theta = R1.norm();
const double r2_theta = R2.norm();
if(r1_theta == 0)
{
// R1 is zero so set to R2
*this = R2;
}
else if(r2_theta == 0)
{
// R2 is zero so set to R1
*this = R1;
}
else
{
const double sin_half_r1 = sin(r1_theta/2.0);
const double cos_half_r1 = cos(r1_theta/2.0);
const double sin_half_r2 = sin(r2_theta/2.0);
const double cos_half_r2 = cos(r2_theta/2.0);
const Vec3D r1_hat = R1/r1_theta;
const Vec3D r2_hat = R2/r2_theta;
const double coshalftheta =
cos_half_r1*cos_half_r2-
sin_half_r1*sin_half_r2*(r1_hat*r2_hat);
const Vec3D axis(r1_hat*sin_half_r1*cos_half_r2+
r2_hat*sin_half_r2*cos_half_r1-
(r1_hat^r2_hat)*sin_half_r1*sin_half_r2);
const double sinhalftheta = axis.norm();
if(sinhalftheta==0)
{
*this = Vec3D(0,0,0);
}
else
{
const double halftheta=atan(sinhalftheta/coshalftheta);
*this = axis/sinhalftheta*halftheta*2.0;
}
}
return *this;
}