ClangFormat: apply to source, most of intern
[blender.git] / intern / iksolver / intern / IK_Math.h
index b10aafa..ce99c39 100644 (file)
@@ -35,219 +35,227 @@ static const double IK_EPSILON = 1e-20;
 
 static inline bool FuzzyZero(double x)
 {
-       return fabs(x) < IK_EPSILON;
+  return fabs(x) < IK_EPSILON;
 }
 
 static inline double Clamp(const double x, const double min, const double max)
 {
-       return (x < min) ? min : (x > max) ? max : x;
+  return (x < min) ? min : (x > max) ? max : x;
 }
 
-static inline Eigen::Matrix3d CreateMatrix(double xx, double xy, double xz,
-                                           double yx, double yy, double yz,
-                                           double zx, double zy, double zz)
+static inline Eigen::Matrix3d CreateMatrix(double xx,
+                                           double xy,
+                                           double xz,
+                                           double yx,
+                                           double yy,
+                                           double yz,
+                                           double zx,
+                                           double zy,
+                                           double zz)
 {
-       Eigen::Matrix3d M;
-       M(0, 0) = xx; M(0, 1) = xy; M(0, 2) = xz;
-       M(1, 0) = yx; M(1, 1) = yy; M(1, 2) = yz;
-       M(2, 0) = zx; M(2, 1) = zy; M(2, 2) = zz;
-       return M;
+  Eigen::Matrix3d M;
+  M(0, 0) = xx;
+  M(0, 1) = xy;
+  M(0, 2) = xz;
+  M(1, 0) = yx;
+  M(1, 1) = yy;
+  M(1, 2) = yz;
+  M(2, 0) = zx;
+  M(2, 1) = zy;
+  M(2, 2) = zz;
+  return M;
 }
 
 static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
 {
-       if (axis == 0)
-               return CreateMatrix(1.0, 0.0, 0.0,
-                                   0.0, cosine, -sine,
-                                   0.0, sine, cosine);
-       else if (axis == 1)
-               return CreateMatrix(cosine, 0.0, sine,
-                                   0.0, 1.0, 0.0,
-                                   -sine, 0.0, cosine);
-       else
-               return CreateMatrix(cosine, -sine, 0.0,
-                                   sine, cosine, 0.0,
-                                   0.0, 0.0, 1.0);
+  if (axis == 0)
+    return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine);
+  else if (axis == 1)
+    return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine);
+  else
+    return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
 }
 
 static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
 {
-       return RotationMatrix(sin(angle), cos(angle), axis);
+  return RotationMatrix(sin(angle), cos(angle), axis);
 }
 
-
-static inline double EulerAngleFromMatrix(const Eigen::Matrix3d& R, int axis)
+static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis)
 {
-       double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
-
-       if (t > 16.0 * IK_EPSILON) {
-               if      (axis == 0) return -atan2(R(1, 2), R(2, 2));
-               else if (axis == 1) return  atan2(-R(0, 2), t);
-               else                return -atan2(R(0, 1), R(0, 0));
-       }
-       else {
-               if      (axis == 0) return -atan2(-R(2, 1), R(1, 1));
-               else if (axis == 1) return  atan2(-R(0, 2), t);
-               else                return 0.0f;
-       }
+  double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
+
+  if (t > 16.0 * IK_EPSILON) {
+    if (axis == 0)
+      return -atan2(R(1, 2), R(2, 2));
+    else if (axis == 1)
+      return atan2(-R(0, 2), t);
+    else
+      return -atan2(R(0, 1), R(0, 0));
+  }
+  else {
+    if (axis == 0)
+      return -atan2(-R(2, 1), R(1, 1));
+    else if (axis == 1)
+      return atan2(-R(0, 2), t);
+    else
+      return 0.0f;
+  }
 }
 
 static inline double safe_acos(double f)
 {
-       // acos that does not return NaN with rounding errors
-       if (f <= -1.0)
-               return M_PI;
-       else if (f >= 1.0)
-               return 0.0;
-       else
-               return acos(f);
+  // acos that does not return NaN with rounding errors
+  if (f <= -1.0)
+    return M_PI;
+  else if (f >= 1.0)
+    return 0.0;
+  else
+    return acos(f);
 }
 
-static inline Eigen::Vector3d normalize(const Eigen::Vector3dv)
+static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v)
 {
-       // a sane normalize function that doesn't give (1, 0, 0) in case
-       // of a zero length vector
-       double len = v.norm();
-       return FuzzyZero(len) ?  Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
+  // a sane normalize function that doesn't give (1, 0, 0) in case
+  // of a zero length vector
+  double len = v.norm();
+  return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
 }
 
-static inline double angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
+static inline double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2)
 {
-       return safe_acos(v1.dot(v2));
+  return safe_acos(v1.dot(v2));
 }
 
-static inline double ComputeTwist(const Eigen::Matrix3dR)
+static inline double ComputeTwist(const Eigen::Matrix3d &R)
 {
-       // qy and qw are the y and w components of the quaternion from R
-       double qy = R(0, 2) - R(2, 0);
-       double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
+  // qy and qw are the y and w components of the quaternion from R
+  double qy = R(0, 2) - R(2, 0);
+  double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
 
-       double tau = 2.0 * atan2(qy, qw);
+  double tau = 2.0 * atan2(qy, qw);
 
-       return tau;
+  return tau;
 }
 
 static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
 {
-       return RotationMatrix(tau, 1);
+  return RotationMatrix(tau, 1);
 }
 
-static inline void RemoveTwist(Eigen::Matrix3dR)
+static inline void RemoveTwist(Eigen::Matrix3d &R)
 {
-       // compute twist parameter
-       double tau = ComputeTwist(R);
+  // compute twist parameter
+  double tau = ComputeTwist(R);
 
-       // compute twist matrix
-       Eigen::Matrix3d T = ComputeTwistMatrix(tau);
+  // compute twist matrix
+  Eigen::Matrix3d T = ComputeTwistMatrix(tau);
 
-       // remove twist
-       R = R * T.transpose();
+  // remove twist
+  R = R * T.transpose();
 }
 
-static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3dR)
+static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
 {
-       // compute twist parameter
-       double tau = ComputeTwist(R);
+  // compute twist parameter
+  double tau = ComputeTwist(R);
 
-       // compute swing parameters
-       double num = 2.0 * (1.0 + R(1, 1));
+  // compute swing parameters
+  double num = 2.0 * (1.0 + R(1, 1));
 
-       // singularity at pi
-       if (fabs(num) < IK_EPSILON)
-               // TODO: this does now rotation of size pi over z axis, but could
-               // be any axis, how to deal with this i'm not sure, maybe don't
-               // enforce limits at all then
-               return Eigen::Vector3d(0.0, tau, 1.0);
+  // singularity at pi
+  if (fabs(num) < IK_EPSILON)
+    // TODO: this does now rotation of size pi over z axis, but could
+    // be any axis, how to deal with this i'm not sure, maybe don't
+    // enforce limits at all then
+    return Eigen::Vector3d(0.0, tau, 1.0);
 
-       num = 1.0 / sqrt(num);
-       double ax = -R(2, 1) * num;
-       double az =  R(0, 1) * num;
+  num = 1.0 / sqrt(num);
+  double ax = -R(2, 1) * num;
+  double az = R(0, 1) * num;
 
-       return Eigen::Vector3d(ax, tau, az);
+  return Eigen::Vector3d(ax, tau, az);
 }
 
 static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
 {
-       // length of (ax, 0, az) = sin(theta/2)
-       double sine2 = ax * ax + az * az;
-       double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
-
-       // compute swing matrix
-       Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
-
-       return S;
-}
-
-static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d& R)
-{
-       Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2),
-                                     R(0, 2) - R(2, 0),
-                                     R(1, 0) - R(0, 1));
-
-       double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
-       double l = delta.norm();
-       
-       if (!FuzzyZero(l))
-               delta *= c / l;
-       
-       return delta;
-}
-
-static inline bool EllipseClamp(double& ax, double& az, double *amin, double *amax)
-{
-       double xlim, zlim, x, z;
-
-       if (ax < 0.0) {
-               x = -ax;
-               xlim = -amin[0];
-       }
-       else {
-               x = ax;
-               xlim = amax[0];
-       }
-
-       if (az < 0.0) {
-               z = -az;
-               zlim = -amin[1];
-       }
-       else {
-               z = az;
-               zlim = amax[1];
-       }
-
-       if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
-               if (x <= xlim && z <= zlim)
-                       return false;
-
-               if (x > xlim)
-                       x = xlim;
-               if (z > zlim)
-                       z = zlim;
-       }
-       else {
-               double invx = 1.0 / (xlim * xlim);
-               double invz = 1.0 / (zlim * zlim);
-
-               if ((x * x * invx + z * z * invz) <= 1.0)
-                       return false;
-
-               if (FuzzyZero(x)) {
-                       x = 0.0;
-                       z = zlim;
-               }
-               else {
-                       double rico = z / x;
-                       double old_x = x;
-                       x = sqrt(1.0 / (invx + invz * rico * rico));
-                       if (old_x < 0.0)
-                               x = -x;
-                       z = rico * x;
-               }
-       }
-
-       ax = (ax < 0.0) ? -x : x;
-       az = (az < 0.0) ? -z : z;
-
-       return true;
+  // length of (ax, 0, az) = sin(theta/2)
+  double sine2 = ax * ax + az * az;
+  double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
+
+  // compute swing matrix
+  Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
+
+  return S;
+}
+
+static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
+{
+  Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
+
+  double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
+  double l = delta.norm();
+
+  if (!FuzzyZero(l))
+    delta *= c / l;
+
+  return delta;
 }
 
+static inline bool EllipseClamp(double &ax, double &az, double *amin, double *amax)
+{
+  double xlim, zlim, x, z;
+
+  if (ax < 0.0) {
+    x = -ax;
+    xlim = -amin[0];
+  }
+  else {
+    x = ax;
+    xlim = amax[0];
+  }
+
+  if (az < 0.0) {
+    z = -az;
+    zlim = -amin[1];
+  }
+  else {
+    z = az;
+    zlim = amax[1];
+  }
+
+  if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
+    if (x <= xlim && z <= zlim)
+      return false;
+
+    if (x > xlim)
+      x = xlim;
+    if (z > zlim)
+      z = zlim;
+  }
+  else {
+    double invx = 1.0 / (xlim * xlim);
+    double invz = 1.0 / (zlim * zlim);
+
+    if ((x * x * invx + z * z * invz) <= 1.0)
+      return false;
+
+    if (FuzzyZero(x)) {
+      x = 0.0;
+      z = zlim;
+    }
+    else {
+      double rico = z / x;
+      double old_x = x;
+      x = sqrt(1.0 / (invx + invz * rico * rico));
+      if (old_x < 0.0)
+        x = -x;
+      z = rico * x;
+    }
+  }
+
+  ax = (ax < 0.0) ? -x : x;
+  az = (az < 0.0) ? -z : z;
+
+  return true;
+}