Changes in src/vector_ops.cpp [42a101:fa5a6a]

File:

• src/vector_ops.cpp (modified) (2 diffs)

src/vector_ops.cpp

@@ -15 +15 @@
 #include "Helpers/fast_functions.hpp"
 #include "Exceptions/LinearDependenceException.hpp"
-#include "Exceptions/SkewException.hpp"
 
 #include <gsl/gsl_linalg.h>
@@ -111 +110 @@
   return true;
 };
+
+/** Rotates the vector relative to the origin around the axis given by \a *axis by an angle of \a alpha.
+ * \param *axis rotation axis
+ * \param alpha rotation angle in radian
+ */
+Vector RotateVector(const Vector &vec,const Vector &axis, const double alpha)
+{
+  Vector a,y;
+  Vector res;
+  // normalise this vector with respect to axis
+  a = vec;
+  a.ProjectOntoPlane(axis);
+  // construct normal vector
+  try {
+    y = Plane(axis,a,0).getNormal();
+  }
+  catch (MathException &excp) {
+    // The normal vector cannot be created if there is linar dependency.
+    // Then the vector to rotate is on the axis and any rotation leads to the vector itself.
+    return vec;
+  }
+  y.Scale(vec.Norm());
+  // scale normal vector by sine and this vector by cosine
+  y.Scale(sin(alpha));
+  a.Scale(cos(alpha));
+  res = vec.Projection(axis);
+  // add scaled normal vector onto this vector
+  res += y;
+  // add part in axis direction
+  res += a;
+  return res;
+};
+
+/** Calculates the intersection of the two lines that are both on the same plane.
+ * This is taken from Weisstein, Eric W. "Line-Line Intersection." From MathWorld--A Wolfram Web Resource. http://mathworld.wolfram.com/Line-LineIntersection.html
+ * \param *out output stream for debugging
+ * \param *Line1a first vector of first line
+ * \param *Line1b second vector of first line
+ * \param *Line2a first vector of second line
+ * \param *Line2b second vector of second line
+ * \return true - \a this will contain the intersection on return, false - lines are parallel
+ */
+Vector GetIntersectionOfTwoLinesOnPlane(const Vector &Line1a, const Vector &Line1b, const Vector &Line2a, const Vector &Line2b)
+{
+  Info FunctionInfo(__func__);
+
+  Vector res;
+
+  auto_ptr<GSLMatrix> M = auto_ptr<GSLMatrix>(new GSLMatrix(4,4));
+
+  M->SetAll(1.);
+  for (int i=0;i<3;i++) {
+    M->Set(0, i, Line1a[i]);
+    M->Set(1, i, Line1b[i]);
+    M->Set(2, i, Line2a[i]);
+    M->Set(3, i, Line2b[i]);
+  }
+
+  //Log() << Verbose(1) << "Coefficent matrix is:" << endl;
+  //for (int i=0;i<4;i++) {
+  //  for (int j=0;j<4;j++)
+  //    cout << "\t" << M->Get(i,j);
+  //  cout << endl;
+  //}
+  if (fabs(M->Determinant()) > MYEPSILON) {
+    Log() << Verbose(1) << "Determinant of coefficient matrix is NOT zero." << endl;
+    throw LinearDependenceException(__FILE__,__LINE__);
+  }
+
+  Log() << Verbose(1) << "INFO: Line1a = " << Line1a << ", Line1b = " << Line1b << ", Line2a = " << Line2a << ", Line2b = " << Line2b << "." << endl;
+
+
+  // constuct a,b,c
+  Vector a = Line1b - Line1a;
+  Vector b = Line2b - Line2a;
+  Vector c = Line2a - Line1a;
+  Vector d = Line2b - Line1b;
+  Log() << Verbose(1) << "INFO: a = " << a << ", b = " << b << ", c = " << c << "." << endl;
+  if ((a.NormSquared() < MYEPSILON) || (b.NormSquared() < MYEPSILON)) {
+   res.Zero();
+   Log() << Verbose(1) << "At least one of the lines is ill-defined, i.e. offset equals second vector." << endl;
+   throw LinearDependenceException(__FILE__,__LINE__);
+  }
+
+  // check for parallelity
+  Vector parallel;
+  double factor = 0.;
+  if (fabs(a.ScalarProduct(b)*a.ScalarProduct(b)/a.NormSquared()/b.NormSquared() - 1.) < MYEPSILON) {
+    parallel = Line1a - Line2a;
+    factor = parallel.ScalarProduct(a)/a.Norm();
+    if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
+      res = Line2a;
+      Log() << Verbose(1) << "Lines conincide." << endl;
+      return res;
+    } else {
+      parallel = Line1a - Line2b;
+      factor = parallel.ScalarProduct(a)/a.Norm();
+      if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
+        res = Line2b;
+        Log() << Verbose(1) << "Lines conincide." << endl;
+        return res;
+      }
+    }
+    Log() << Verbose(1) << "Lines are parallel." << endl;
+    res.Zero();
+    throw LinearDependenceException(__FILE__,__LINE__);
+  }
+
+  // obtain s
+  double s;
+  Vector temp1, temp2;
+  temp1 = c;
+  temp1.VectorProduct(b);
+  temp2 = a;
+  temp2.VectorProduct(b);
+  Log() << Verbose(1) << "INFO: temp1 = " << temp1 << ", temp2 = " << temp2 << "." << endl;
+  if (fabs(temp2.NormSquared()) > MYEPSILON)
+    s = temp1.ScalarProduct(temp2)/temp2.NormSquared();
+  else
+    s = 0.;
+  Log() << Verbose(1) << "Factor s is " << temp1.ScalarProduct(temp2) << "/" << temp2.NormSquared() << " = " << s << "." << endl;
+
+  // construct intersection
+  res = a;
+  res.Scale(s);
+  res += Line1a;
+  Log() << Verbose(1) << "Intersection is at " << res << "." << endl;
+
+  return res;
+};