Пример #1
0
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);
}
Пример #2
0
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;
}