diff --git a/modules/geom/src/composite3.cc b/modules/geom/src/composite3.cc index 686e843e150c15729acd330cd8bc8d4112249c7f..751901b303d3b25f2b863c6767a7b21ff3343948 100644 --- a/modules/geom/src/composite3.cc +++ b/modules/geom/src/composite3.cc @@ -224,25 +224,12 @@ Vec3 Rotation3::GetRotationAxis() const } Real Rotation3::GetRotationAngle() const { - return 2.0*acos(q_.GetAngle()); + //return 2.0*acos(q_.GetAngle()); + return q_.GetAngle(); } Mat3 Rotation3::GetRotationMatrix() const { - Real ww = q_.GetAngle()*q_.GetAngle(); - Real wx = q_.GetAngle()*q_.GetAxis().GetX(); - Real wy = q_.GetAngle()*q_.GetAxis().GetY(); - Real wz = q_.GetAngle()*q_.GetAxis().GetZ(); - Real xx = q_.GetAxis().GetX()*q_.GetAxis().GetX(); - Real xy = q_.GetAxis().GetX()*q_.GetAxis().GetY(); - Real xz = q_.GetAxis().GetX()*q_.GetAxis().GetZ(); - Real yy = q_.GetAxis().GetY()*q_.GetAxis().GetY(); - Real yz = q_.GetAxis().GetY()*q_.GetAxis().GetZ(); - Real zz = q_.GetAxis().GetZ()*q_.GetAxis().GetZ(); - - - return Mat3((ww+xx-yy-zz), 2.0*(-wz+xy), 2.0*(wy+xz), - 2.0*(wz+xy), (ww-xx+yy-zz), 2.0*(-wx+yz), - 2.0*(-wy+xz), 2.0*(wx+yz),(ww-xx-yy+zz)); + return q_.ToRotationMatrix(); } void Rotation3::SetOrigin(const Vec3& o) @@ -299,7 +286,8 @@ Quat Rotation3::generate_from_eulers(Real phi, Real theta, Real psi) } Quat Rotation3::generate_from_axis_angle(const Vec3& axis, Real angle) { - return Quat(cos(angle/2.0),sin(angle/2.0)*Normalize(axis)); + //return Quat(cos(angle/2.0),sin(angle/2.0)*Normalize(axis)); + return Quat(angle, axis); } /* diff --git a/modules/geom/src/quat.cc b/modules/geom/src/quat.cc index b923a668008fbe908f55bd1a10a584598ea930de..f9b852cae7799478c8f0ca3bfb599c3b80821421 100644 --- a/modules/geom/src/quat.cc +++ b/modules/geom/src/quat.cc @@ -287,7 +287,8 @@ Vec3 Quat::GetAxis() const Real Quat::GetAngle() const { - Real ww = std::acos(w/Length(Vec3(x,y,z))); + //Real ww = std::acos(w/Length(Vec3(x,y,z))); + Real ww = 2.0*std::acos(w); return ww; }